SheepShaver/src/ether.cpp

/*
 *  ether.cpp - SheepShaver Ethernet Device Driver (DLPI)
 *
 *  SheepShaver (C) 1997-2005 Marc Hellwig and Christian Bauer
 *
 *  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
 */

/*
 * TODO
 * - 802.2 TEST/XID
 * - MIB statistics
 */

#include <string.h>

#include "sysdeps.h"
#include "cpu_emulation.h"
#include "ether.h"
#include "ether_defs.h"
#include "macos_util.h"
#include "emul_op.h"
#include "main.h"

#define DEBUG 0
#include "debug.h"

#if DIRECT_ADDRESSING
#warning "This code is not direct addressing clean"
#endif

// Packet types
enum {
        kPktDIX                         = 0,
        kPkt8022SAP                     = 1,
        kPkt8022GroupSAP        = 2,
        kPkt8022SNAP            = 3,
        kPktIPX                         = 4,
        kPktUnknown                     = 5
};


/*
 *  Stream private data structure
 */

static const int kGroupSAPMapSize = 128/32;     // Number of 32-bit values we need for 128 bits 
static const int kGSshift = 6;
static const int kGSmask = 0x1F;

struct multicast_node {
        nw_multicast_node_p next;
        uint8 addr[kEnetPhysicalAddressLength];
};

struct DLPIStream {
        void SetGroupSAP(uint8 sap) 
        {
                group_sap[sap >> kGSshift] |= (1L << ((sap >> 1) & kGSmask));
        }

        void ClearGroupSAP(uint8 sap)
        {
                group_sap[sap >> kGSshift] &= ~(1L << ((sap >> 1) & kGSmask));
        }

        void ClearAllGroupSAPs(void)
        {
                for (int i=0; i<kGroupSAPMapSize; i++)
                        group_sap[i] = 0;
        }

        bool TestGroupSAP(uint8 sap) 
        {
                return group_sap[sap >> kGSshift] & (1L << ((sap >> 1) & kGSmask));
        }

        void AddMulticast(uint8 *addr)
        {
                multicast_node *n = (multicast_node *)Mac2HostAddr(Mac_sysalloc(sizeof(multicast_node)));
                memcpy(n->addr, addr, kEnetPhysicalAddressLength);
                n->next = multicast_list;
                multicast_list = n;
        }

        void RemoveMulticast(uint8 *addr)
        {
                multicast_node *p = multicast_list;
                while (p) {
                        if (memcmp(addr, p->addr, kEnetPhysicalAddressLength) == 0)
                                goto found;
                        p = p->next;
                }
                return;
        found:
                multicast_node *q = (multicast_node *)&multicast_list;
                while (q) {
                        if (q->next == p) {
                                q->next = p->next;
                                Mac_sysfree(Host2MacAddr((uint8 *)p));
                                return;
                        }
                        q = q->next;
                }
        }

        uint8 *IsMulticastRegistered(uint8 *addr)
        {
                multicast_node *n = multicast_list;
                while (n) {
                        if (memcmp(addr, n->addr, kEnetPhysicalAddressLength) == 0)
                                return n->addr;
                        n = n->next;
                }
                return NULL;
        }

        nw_uint32 minor_num;                                    // Minor device number of this stream
        nw_uint32 dlpi_state;                                   // DLPI state of this stream
        nw_uint32 flags;                                                // Flags
        nw_uint16 dlsap;                                                // SAP bound to this stream
        nw_bool framing_8022;                                   // Using 802.2 framing? This is only used to report the MAC type for DL_INFO_ACK and can be set with an ioctl() call
        nw_queue_p rdq;                                                 // Read queue for this stream
        nw_uint32 group_sap[kGroupSAPMapSize];  // Map of bound group SAPs
        uint8 snap[k8022SNAPLength];                    // SNAP bound to this stream
        nw_multicast_node_p multicast_list;             // List of enabled multicast addresses
};

// Hack to make DLPIStream list initialization early to NULL (do we really need this?)
struct DLPIStreamInit {
        DLPIStreamInit(nw_DLPIStream_p *dlpi_stream_p) { *dlpi_stream_p = NULL; }
};

// Stream flags
enum {
        kSnapStream                             = 0x00000001,
        kAcceptMulticasts               = 0x00000002,
        kAcceptAll8022Packets   = 0x00000004,
        kFastPathMode                   = 0x00000008
};

// List of opened streams (used internally by OpenTransport)
static nw_DLPIStream_p dlpi_stream_list;
static DLPIStreamInit dlpi_stream_init(&dlpi_stream_list);

// Are we open?
bool ether_driver_opened = false;

// Our ethernet hardware address
static uint8 hardware_address[6] = {0, 0, 0, 0, 0, 0};

// Statistics
int32 num_wput = 0;
int32 num_error_acks = 0;
int32 num_tx_packets = 0;
int32 num_tx_raw_packets = 0;
int32 num_tx_normal_packets = 0;
int32 num_tx_buffer_full = 0;
int32 num_rx_packets = 0;
int32 num_ether_irq = 0;
int32 num_unitdata_ind = 0;
int32 num_rx_fastpath = 0;
int32 num_rx_no_mem = 0;
int32 num_rx_dropped = 0;
int32 num_rx_stream_not_ready = 0;
int32 num_rx_no_unitdata_mem = 0;


// Function pointers of imported functions
typedef mblk_t *(*allocb_ptr)(size_t size, int pri);
static uint32 allocb_tvect = 0;
mblk_t *allocb(size_t arg1, int arg2)
{
        return (mblk_t *)Mac2HostAddr(CallMacOS2(allocb_ptr, allocb_tvect, arg1, arg2));
}
typedef void (*freeb_ptr)(mblk_t *);
static uint32 freeb_tvect = 0;
static inline void freeb(mblk_t *arg1)
{
        CallMacOS1(freeb_ptr, freeb_tvect, arg1);
}
typedef int16 (*freemsg_ptr)(mblk_t *);
static uint32 freemsg_tvect = 0;
static inline int16 freemsg(mblk_t *arg1)
{
        return (int16)CallMacOS1(freemsg_ptr, freemsg_tvect, arg1);
}
typedef mblk_t *(*copyb_ptr)(mblk_t *);
static uint32 copyb_tvect = 0;
static inline mblk_t *copyb(mblk_t *arg1)
{
        return (mblk_t *)Mac2HostAddr(CallMacOS1(copyb_ptr, copyb_tvect, arg1));
}
typedef mblk_t *(*dupmsg_ptr)(mblk_t *);
static uint32 dupmsg_tvect = 0;
static inline mblk_t *dupmsg(mblk_t *arg1)
{
        return (mblk_t *)Mac2HostAddr(CallMacOS1(dupmsg_ptr, dupmsg_tvect, arg1));
}
typedef mblk_t *(*getq_ptr)(queue_t *);
static uint32 getq_tvect = 0;
static inline mblk_t *getq(queue_t *arg1)
{
        return (mblk_t *)Mac2HostAddr(CallMacOS1(getq_ptr, getq_tvect, arg1));
}
typedef int (*putq_ptr)(queue_t *, mblk_t *);
static uint32 putq_tvect = 0;
static inline int putq(queue_t *arg1, mblk_t *arg2)
{
        return (int)CallMacOS2(putq_ptr, putq_tvect, arg1, arg2);
}
typedef int (*putnext_ptr)(queue_t *,  mblk_t *);
static uint32 putnext_tvect = 0;
static inline int putnext(queue_t *arg1, mblk_t *arg2)
{
        return (int)CallMacOS2(putnext_ptr, putnext_tvect, arg1, arg2);
}
typedef int (*putnextctl1_ptr)(queue_t *, int type, int c);
static uint32 putnextctl1_tvect = 0;
static inline int putnextctl1(queue_t *arg1, int arg2, int arg3)
{
        return (int)CallMacOS3(putnextctl1_ptr, putnextctl1_tvect, arg1, arg2, arg3);
}
typedef int (*canputnext_ptr)(queue_t *);
static uint32 canputnext_tvect = 0;
static inline int canputnext(queue_t *arg1)
{
        return (int)CallMacOS1(canputnext_ptr, canputnext_tvect, arg1);
}
typedef int (*qreply_ptr)(queue_t *, mblk_t *);
static uint32 qreply_tvect = 0;
static inline int qreply(queue_t *arg1, mblk_t *arg2)
{
        return (int)CallMacOS2(qreply_ptr, qreply_tvect, arg1, arg2);
}
typedef void (*flushq_ptr)(queue_t *, int flag);
static uint32 flushq_tvect = 0;
static inline void flushq(queue_t *arg1, int arg2)
{
        CallMacOS2(flushq_ptr, flushq_tvect, arg1, arg2);
}
typedef int (*msgdsize_ptr)(const mblk_t *);
static uint32 msgdsize_tvect = 0;
static inline int msgdsize(const mblk_t *arg1)
{
        return (int)CallMacOS1(msgdsize_ptr, msgdsize_tvect, arg1);
}
typedef void (*otenterint_ptr)(void);
static uint32 otenterint_tvect = 0;
void OTEnterInterrupt(void)
{
        CallMacOS(otenterint_ptr, otenterint_tvect);
}
typedef void (*otleaveint_ptr)(void);
static uint32 otleaveint_tvect = 0;
void OTLeaveInterrupt(void)
{
        CallMacOS(otleaveint_ptr, otleaveint_tvect);
}
typedef int (*mi_open_comm_ptr)(DLPIStream **mi_opp_orig, size_t size, queue_t *q, void *dev, int flag, int sflag, void *credp);
static uint32 mi_open_comm_tvect = 0;
static inline int mi_open_comm(DLPIStream **arg1, size_t arg2, queue_t *arg3, void *arg4, int arg5, int arg6, void *arg7)
{
        return (int)CallMacOS7(mi_open_comm_ptr, mi_open_comm_tvect, arg1, arg2, arg3, arg4, arg5, arg6, arg7);
}
typedef int (*mi_close_comm_ptr)(DLPIStream **mi_opp_orig, queue_t *q);
static uint32 mi_close_comm_tvect = 0;
static inline int mi_close_comm(DLPIStream **arg1, queue_t *arg2)
{
        return (int)CallMacOS2(mi_close_comm_ptr, mi_close_comm_tvect, arg1, arg2);
}
typedef DLPIStream *(*mi_next_ptr_ptr)(DLPIStream *);
static uint32 mi_next_ptr_tvect = 0;
static inline DLPIStream *mi_next_ptr(DLPIStream *arg1)
{
        return (DLPIStream *)Mac2HostAddr(CallMacOS1(mi_next_ptr_ptr, mi_next_ptr_tvect, arg1));
}


// Prototypes
static void ether_ioctl(DLPIStream *the_stream, queue_t* q, mblk_t* mp);
static void ether_flush(queue_t* q, mblk_t* mp);
static mblk_t *build_tx_packet_header(DLPIStream *the_stream, mblk_t *mp, bool fast_path);
static void transmit_packet(mblk_t *mp);
static void DLPI_error_ack(DLPIStream *the_stream, queue_t *q, mblk_t *ack_mp, uint32 prim, uint32 err, uint32 uerr);
static void DLPI_ok_ack(DLPIStream *the_stream, queue_t *q, mblk_t *ack_mp, uint32 prim);
static void DLPI_info(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_phys_addr(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_bind(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_unbind(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_subs_bind(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_subs_unbind(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_enable_multi(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_disable_multi(DLPIStream *the_stream, queue_t *q, mblk_t *mp);
static void DLPI_unit_data(DLPIStream *the_stream, queue_t *q, mblk_t *mp);


/*
 *  Initialize ethernet stream module
 */

uint8 InitStreamModule(void *theID)
{
        D(bug("InitStreamModule\n"));

        // Don't re-open if already open
        if (ether_driver_opened)
                return true;
        ether_driver_opened = false;

        // Import functions from OTKernelLib
        allocb_tvect = FindLibSymbol("\013OTKernelLib", "\006allocb");
        D(bug("allocb TVECT at %08lx\n", allocb_tvect));
        if (allocb_tvect == 0)
                return false;
        freeb_tvect = FindLibSymbol("\013OTKernelLib", "\005freeb");
        D(bug("freeb TVECT at %08lx\n", freeb_tvect));
        if (freeb_tvect == 0)
                return false;
        freemsg_tvect = FindLibSymbol("\013OTKernelLib", "\007freemsg");
        D(bug("freemsg TVECT at %08lx\n", freemsg_tvect));
        if (freemsg_tvect == 0)
                return false;
        copyb_tvect = FindLibSymbol("\013OTKernelLib", "\005copyb");
        D(bug("copyb TVECT at %08lx\n", copyb_tvect));
        if (copyb_tvect == 0)
                return false;
        dupmsg_tvect = FindLibSymbol("\013OTKernelLib", "\006dupmsg");
        D(bug("dupmsg TVECT at %08lx\n", dupmsg_tvect));
        if (dupmsg_tvect == 0)
                return false;
        getq_tvect = FindLibSymbol("\013OTKernelLib", "\004getq");
        D(bug("getq TVECT at %08lx\n", getq_tvect));
        if (getq_tvect == 0)
                return false;
        putq_tvect = FindLibSymbol("\013OTKernelLib", "\004putq");
        D(bug("putq TVECT at %08lx\n", putq_tvect));
        if (putq_tvect == 0)
                return false;
        putnext_tvect = FindLibSymbol("\013OTKernelLib", "\007putnext");
        D(bug("putnext TVECT at %08lx\n", putnext_tvect));
        if (putnext_tvect == 0)
                return false;
        putnextctl1_tvect = FindLibSymbol("\013OTKernelLib", "\013putnextctl1");
        D(bug("putnextctl1 TVECT at %08lx\n", putnextctl1_tvect));
        if (putnextctl1_tvect == 0)
                return false;
        canputnext_tvect = FindLibSymbol("\013OTKernelLib", "\012canputnext");
        D(bug("canputnext TVECT at %08lx\n", canputnext_tvect));
        if (canputnext_tvect == 0)
                return false;
        qreply_tvect = FindLibSymbol("\013OTKernelLib", "\006qreply");
        D(bug("qreply TVECT at %08lx\n", qreply_tvect));
        if (qreply_tvect == 0)
                return false;
        flushq_tvect = FindLibSymbol("\013OTKernelLib", "\006flushq");
        D(bug("flushq TVECT at %08lx\n", flushq_tvect));
        if (flushq_tvect == 0)
                return false;
        msgdsize_tvect = FindLibSymbol("\013OTKernelLib", "\010msgdsize");
        D(bug("msgdsize TVECT at %08lx\n", msgdsize_tvect));
        if (msgdsize_tvect == 0)
                return false;
        otenterint_tvect = FindLibSymbol("\017OTKernelUtilLib", "\020OTEnterInterrupt");
        D(bug("OTEnterInterrupt TVECT at %08lx\n", otenterint_tvect));
        if (otenterint_tvect == 0)
                return false;
        otleaveint_tvect = FindLibSymbol("\017OTKernelUtilLib", "\020OTLeaveInterrupt");
        D(bug("OTLeaveInterrupt TVECT at %08lx\n", otleaveint_tvect));
        if (otleaveint_tvect == 0)
                return false;
        mi_open_comm_tvect = FindLibSymbol("\013OTKernelLib", "\014mi_open_comm");
        D(bug("mi_open_comm TVECT at %08lx\n", mi_open_comm_tvect));
        if (mi_open_comm_tvect == 0)
                return false;
        mi_close_comm_tvect = FindLibSymbol("\013OTKernelLib", "\015mi_close_comm");
        D(bug("mi_close_comm TVECT at %08lx\n", mi_close_comm_tvect));
        if (mi_close_comm_tvect == 0)
                return false;
        mi_next_ptr_tvect = FindLibSymbol("\013OTKernelLib", "\013mi_next_ptr");
        D(bug("mi_next_ptr TVECT at %08lx\n", mi_next_ptr_tvect));
        if (mi_next_ptr_tvect == 0)
                return false;

        // Initialize stream list (which might be leftover)
        dlpi_stream_list = NULL;

        // Ask add-on for ethernet hardware address
        AO_get_ethernet_address(Host2MacAddr(hardware_address));

        // Yes, we're open
        ether_driver_opened = true;
        return true;
}


/*
 *  Terminate ethernet stream module
 */

void TerminateStreamModule(void)
{
        D(bug("TerminateStreamModule\n"));

        // This happens sometimes. I don't know why.
        if (dlpi_stream_list != NULL)
                printf("FATAL: TerminateStreamModule() called, but streams still open\n");

        // Sorry, we're closed
        ether_driver_opened = false;
}


/*
 *  Open new stream
 */

int ether_open(queue_t *rdq, void *dev, int flag, int sflag, void *creds)
{
        D(bug("ether_open(%p,%p,%d,%d,%p)\n", rdq, dev, flag, sflag, creds));

        // Return if driver was closed
        if (!ether_driver_opened) {
                printf("FATAL: ether_open(): Ethernet driver not opened\n");
                return MAC_ENXIO;
        }

        // If we're being reopened, just return
        if (rdq->q_ptr != NULL)
                return 0;

        // Allocate DLPIStream structure
        int err = mi_open_comm((DLPIStream **)&dlpi_stream_list, sizeof(DLPIStream), rdq, dev, flag, sflag, creds);
        if (err)
                return err;
        DLPIStream *the_stream = (DLPIStream *)rdq->q_ptr;
        the_stream->rdq = rdq;
        the_stream->dlpi_state = DL_UNBOUND;
        the_stream->flags = 0;
        the_stream->dlsap = 0;
        the_stream->framing_8022 = false;
        the_stream->multicast_list = NULL;
        return 0;
}


/*
 *  Close stream
 */

int ether_close(queue_t *rdq, int flag, void *creds)
{
        D(bug("ether_close(%p,%d,%p)\n", rdq, flag, creds));

        // Return if driver was closed
        if (!ether_driver_opened) {
                printf("FATAL: ether_close(): Ethernet driver not opened\n");
                return MAC_ENXIO;
        }

        // Get stream
        DLPIStream *the_stream = (DLPIStream *)rdq->q_ptr;

        // Don't close if never opened
        if (the_stream == NULL)
                return 0;

        // Disable all registered multicast addresses
        while (the_stream->multicast_list) {
                AO_disable_multicast(Host2MacAddr(the_stream->multicast_list->addr));
                the_stream->RemoveMulticast(the_stream->multicast_list->addr);
        }
        the_stream->multicast_list = NULL;

        // Delete the DLPIStream
        return mi_close_comm((DLPIStream **)&dlpi_stream_list, rdq);
}


/*
 *  Put something on the write queue
 */

int ether_wput(queue_t *q, mblk_t *mp)
{
        D(bug("ether_wput(%p,%p)\n", q, mp));

        // Return if driver was closed
        if (!ether_driver_opened) {
                printf("FATAL: ether_wput(): Ethernet driver not opened\n");
                return MAC_ENXIO;
        }

        // Get stream
        DLPIStream *the_stream = (DLPIStream *)q->q_ptr;
        if (the_stream == NULL)
                return MAC_ENXIO;

        D(bug(" db_type %d\n", (int)mp->b_datap->db_type));
        switch (mp->b_datap->db_type) {

                case M_DATA:
                        // Transmit raw packet
                        D(bug(" raw packet\n"));
                        num_tx_raw_packets++;
                        transmit_packet(mp);
                        break;

                case M_PROTO:
                case M_PCPROTO: {
                        union DL_primitives *dlp = (union DL_primitives *)(void *)mp->b_rptr;
                        uint32 prim = dlp->dl_primitive;
                        D(bug(" dl_primitive %d\n", prim));
                        switch (prim) {
                                case DL_UNITDATA_REQ:
                                        // Transmit normal packet
                                        num_tx_normal_packets++;
                                        DLPI_unit_data(the_stream, q, mp);
                                        break;

                                case DL_INFO_REQ:
                                        DLPI_info(the_stream, q, mp);
                                        break;
        
                                case DL_PHYS_ADDR_REQ:
                                        DLPI_phys_addr(the_stream, q, mp);
                                        break;
        
                                case DL_BIND_REQ:
                                        DLPI_bind(the_stream, q, mp);
                                        break;
        
                                case DL_UNBIND_REQ:
                                        DLPI_unbind(the_stream, q, mp);
                                        break;
        
                                case DL_SUBS_BIND_REQ:
                                        DLPI_subs_bind(the_stream, q, mp);
                                        break;
        
                                case DL_SUBS_UNBIND_REQ:
                                        DLPI_subs_unbind(the_stream, q, mp);
                                        break;
        
                                case DL_ENABMULTI_REQ:
                                        DLPI_enable_multi(the_stream, q, mp);
                                        break;
        
                                case DL_DISABMULTI_REQ:
                                        DLPI_disable_multi(the_stream, q, mp);
                                        break;
        
                                default:
                                        D(bug("WARNING: ether_wsrv(): Unknown primitive\n"));
                                        DLPI_error_ack(the_stream, q, mp, prim, DL_NOTSUPPORTED, 0);
                                        break;
                        }
                        break;
                }

                case M_IOCTL:
                        ether_ioctl(the_stream, q, mp);
                        break;

                case M_FLUSH:
                        ether_flush(q, mp);
                        break;

                default:
                        D(bug("WARNING: ether_wput(): Unknown message type\n"));
                        freemsg(mp);
                        break;
        }
        num_wput++;
        return 0;
}


/*
 *  Dequeue and process messages from the read queue
 */

int ether_rsrv(queue_t *q)
{
        mblk_t *mp;
        while ((mp = getq(q)) != NULL) {
                if (canputnext(q))
                        putnext(q, mp);
                else {
                        freemsg(mp);
                        flushq(q, FLUSHDATA);
                        break;
                }
        }
        return 0;
}


/*
 *  Handle ioctl calls
 */

static void ether_ioctl(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        struct iocblk *ioc = (struct iocblk *)(void *)mp->b_rptr;
        D(bug(" ether_ioctl(%p,%p) cmd %d\n", q, mp, (int)ioc->ioc_cmd));

        switch (ioc->ioc_cmd) {

                case I_OTSetFramingType: {      // Toggles what the general info primitive returns for dl_mac_type in dl_info_ack_t structure
                        mblk_t *info_mp = mp->b_cont;
                        if (info_mp == NULL || ((info_mp->b_wptr - info_mp->b_rptr) != sizeof(uint32))) {
                                ioc->ioc_error = MAC_EINVAL;
                                goto ioctl_error;
                        }
                        uint32 framing_type = ntohl(*(uint32 *)(void *)info_mp->b_rptr);
                        D(bug("  I_OTSetFramingType type %d\n", framing_type));
                        if (framing_type != kOTGetFramingValue)
                                the_stream->framing_8022 = (framing_type == kOTFraming8022);
                        mp->b_cont = NULL;
                        freemsg(info_mp);
                        if (the_stream->framing_8022)
                                ioc->ioc_rval = kOTFraming8022;
                        else
                                ioc->ioc_rval = kOTFramingEthernet;
                        goto ioctl_ok;
                }

                case DL_IOC_HDR_INFO: {         // Special Mentat call, for fast transmits
                        D(bug("  DL_IOC_HDR_INFO\n"));
                        mblk_t *info_mp = mp->b_cont;

                        // Copy DL_UNITDATA_REQ block
                        mblk_t *unitdata_mp = copyb(info_mp);
                        if (unitdata_mp == NULL) {
                                ioc->ioc_error = MAC_ENOMEM;
                                goto ioctl_error;
                        }
                        unitdata_mp->b_datap->db_type = M_PROTO;

                        // Construct header (converts DL_UNITDATA_REQ -> M_DATA)
                        mblk_t *header_mp = build_tx_packet_header(the_stream, unitdata_mp, true);
                        
                        if (header_mp == NULL) {
                                // Could not allocate a message block large enough
                                ioc->ioc_error = MAC_ENOMEM;
                                goto ioctl_error;
                        }

                        // Attach header block at the end
                        mp->b_cont->b_cont = header_mp;
                        the_stream->flags |= kFastPathMode;
                        goto ioctl_ok;
                }

                case I_OTSetRawMode: {
                        mblk_t *info_mp = mp->b_cont;
                        dl_recv_control_t *dlrc;
                        if (info_mp == NULL || ((info_mp->b_wptr - info_mp->b_rptr) != sizeof(dlrc->dl_primitive))) {
                                ioc->ioc_error = MAC_EINVAL;
                                goto ioctl_error;
                        }
                        dlrc = (dl_recv_control_t *)(void *)info_mp->b_rptr;
                        D(bug("  I_OTSetRawMode primitive %d\n", (int)dlrc->dl_primitive));
                        ioc->ioc_error = MAC_EINVAL;
                        goto ioctl_error;
                }

                default:
                        D(bug("WARNING: Unknown ether_ioctl() call\n"));
                        ioc->ioc_error = MAC_EINVAL;
                        goto ioctl_error;
        }

ioctl_ok:
        ioc->ioc_count = 0;
        for (mblk_t *mp1 = mp; (mp1 = mp1->b_cont) != NULL;)
                ioc->ioc_count += mp1->b_wptr - mp1->b_rptr;
        ioc->ioc_error = 0;
        mp->b_datap->db_type = M_IOCACK;
        qreply(q, mp);
        return;

ioctl_error:
        mp->b_datap->db_type = M_IOCNAK;
        qreply(q, mp);
        return;
}


/*
 *  Flush call, send it up to the read side of the stream
 */

static void ether_flush(queue_t* q, mblk_t* mp)
{
        D(bug(" ether_flush(%p,%p)\n", q, mp));

        uint8 *rptr = mp->b_rptr;
        if (*rptr & FLUSHW)
                flushq(q, FLUSHALL);
        if (*rptr & FLUSHR) {
                flushq(RD(q), FLUSHALL);
                *rptr &= ~FLUSHW;
                qreply(q, mp);
        } else
                freemsg(mp);
}


/*
 *  Classify packet into the different types of protocols
 */

static uint16 classify_packet_type(uint16 primarySAP, uint16 secondarySAP)
{
        if (primarySAP >= kMinDIXSAP)           
                return kPktDIX;

        if ((primarySAP == kIPXSAP) && (secondarySAP == kIPXSAP))
                return kPktIPX;

        if (primarySAP == kSNAPSAP)
                return kPkt8022SNAP;

        if (primarySAP <= k8022GlobalSAP)
                return kPkt8022SAP;

        return kPktUnknown;
}


/*
 *  Check if the address is a multicast, broadcast or standard address
 */

static int32 get_address_type(uint8 *addr)
{
        if (addr[0] & 1) {      // Multicast/broadcast flag
                if (OTIs48BitBroadcastAddress(addr))
                        return keaBroadcast;
                else
                        return keaMulticast;
        } else
                return keaStandardAddress;
}


/*
 *  Reuse a message block, make room for more data
 */

static mblk_t *reuse_message_block(mblk_t *mp, uint16 needed_size)
{
        mblk_t *nmp;

        if ((mp->b_datap->db_ref == 1) && ((mp->b_datap->db_lim - mp->b_datap->db_base) >= needed_size)) {
                mp->b_datap->db_type = M_DATA;
                mp->b_rptr = mp->b_datap->db_base;
                mp->b_wptr = mp->b_datap->db_base + needed_size;
        } else {
                nmp = mp->b_cont;       // Grab the M_DATA blocks
                mp->b_cont = NULL;      // Detach the M_(PC)PROTO
                freemsg(mp);            // Free the M_(PC)PROTO
                mp = nmp;                       // Point to the M_DATA blocks
        
                // Try to get space on the first M_DATA block
                if (mp && (mp->b_datap->db_ref == 1) && ((mp->b_rptr - mp->b_datap->db_base) >= needed_size))
                        mp->b_rptr -= needed_size;
                else {
                        // Try to allocate a new message
                        if ((nmp = allocb(needed_size, BPRI_HI)) == NULL) {
                                // Could not get a new message block so lets forget about the message altogether
                                freemsg(mp);                    // Free the original M_DATA portion of the message
                                mp = NULL;                              // Indicates the reuse failed
                        } else {
                                nmp->b_cont = mp;               // Attach the new message block as the head
                                nmp->b_wptr += needed_size;
                                mp = nmp;                               
                        } 
                }
        }

        return mp;
}


/*
 *  Built header for packet to be transmitted (convert DL_UNITDATA_REQ -> M_DATA)
 *  The passed-in message has the header info in the first message block and the data
 *  in the following blocks
 */

static mblk_t *build_tx_packet_header(DLPIStream *the_stream, mblk_t *mp, bool fast_path)
{
        // Only handle unit_data requests
        dl_unitdata_req_t *req = (dl_unitdata_req_t *)(void *)mp->b_rptr;
        if (req->dl_primitive != DL_UNITDATA_REQ) {
                freemsg(mp);
                return NULL;
        }

        // Extract destination address and its length
        uint8 *destAddrOrig = ((uint8 *)req) + req->dl_dest_addr_offset;
        uint32 destAddrLen = req->dl_dest_addr_length;
        uint8 ctrl = 0x03;

        // Extract DLSAP
        uint16 dlsap;
        switch (destAddrLen) {
                case kEnetPhysicalAddressLength:        
                        dlsap = the_stream->dlsap;
                        break;
                case kEnetAndSAPAddressLength:  
                        dlsap = ntohs(*(uint16 *)(destAddrOrig + kEnetPhysicalAddressLength));
                        break;
                case kEnetPhysicalAddressLength + k8022DLSAPLength + k8022SNAPLength:   // SNAP SAP
                        dlsap = ntohs(*(uint16 *)(destAddrOrig + kEnetPhysicalAddressLength));
                        break;
                default:
                        dlsap = the_stream->dlsap;
                        break;
        }

        // Extract data size (excluding header info) and packet type
        uint16 datasize = msgdsize(mp);
        uint16 packetType = classify_packet_type(the_stream->dlsap, dlsap);

        // Calculate header size and protocol type/size field
        uint16 hdrsize, proto;
        switch (packetType) {
                case kPktDIX:
                        hdrsize = kEnetPacketHeaderLength; 
                        proto = dlsap;
                        break;
                case kPkt8022SAP:
                        hdrsize = kEnetPacketHeaderLength + k8022BasicHeaderLength;
                        if (fast_path)
                                proto = 0;
                        else
                                proto = datasize + k8022BasicHeaderLength;
                        break;
                case kPkt8022SNAP:
                        hdrsize = kEnetPacketHeaderLength + k8022SNAPHeaderLength;
                        if (fast_path)
                                proto = 0;              
                        else
                                proto = datasize + k8022SNAPHeaderLength;
                        break;
                case kPktIPX:
                        hdrsize = kEnetPacketHeaderLength;
                        if (fast_path)
                                proto = 0;
                        else
                                proto = datasize;               
                        break;
                default:
                        hdrsize = kEnetPacketHeaderLength; 
                        proto = dlsap;
                        break;
        }

        // We need to copy the dest address info in the message before we can reuse it
        uint8 destAddrCopy[kMaxBoundAddrLength];
        memcpy(destAddrCopy, destAddrOrig, destAddrLen);

        // Resize header info in message block
        if ((mp = reuse_message_block(mp, hdrsize)) == NULL)
                return NULL;
        struct T8022FullPacketHeader *packetHeader = (struct T8022FullPacketHeader *)(void *)mp->b_rptr;

        // Set protocol type/size field
        packetHeader->fEnetPart.fProto = proto;

        // Set destination ethernet address
        OTCopy48BitAddress(destAddrCopy, packetHeader->fEnetPart.fDestAddr);

        // Set other header fields
        switch (packetType) {
                case kPkt8022SAP:
                        packetHeader->f8022Part.fDSAP = (uint8)dlsap;
                        packetHeader->f8022Part.fSSAP = (uint8)the_stream->dlsap;
                        packetHeader->f8022Part.fCtrl = ctrl;
                        break;
                case kPkt8022SNAP: {
                        uint8 *snapStart;
                        packetHeader->f8022Part.fDSAP = (uint8)dlsap;
                        packetHeader->f8022Part.fSSAP = (uint8)the_stream->dlsap;
                        packetHeader->f8022Part.fCtrl = ctrl;
                        if (destAddrLen >= kEnetAndSAPAddressLength + k8022SNAPLength)
                                snapStart = destAddrCopy + kEnetAndSAPAddressLength;
                        else
                                snapStart = the_stream->snap;
                        OTCopy8022SNAP(snapStart, packetHeader->f8022Part.fSNAP);
                        break;
                }
        }

        // Return updated message
        return mp;
}


/*
 *  Transmit packet
 */

static void transmit_packet(mblk_t *mp)
{
        EnetPacketHeader *enetHeader = (EnetPacketHeader *)(void *)mp->b_rptr;

        // Fill in length in 802.3 packets
        if (enetHeader->fProto == 0)
                enetHeader->fProto = msgdsize(mp) - sizeof(EnetPacketHeader);

        // Fill in ethernet source address
        OTCopy48BitAddress(hardware_address, enetHeader->fSourceAddr); 

        // Tell add-on to transmit packet
        AO_transmit_packet(Host2MacAddr((uint8 *)mp));
        freemsg(mp);
}


/*
 *  Handle incoming packet (one stream), construct DL_UNITDATA_IND message
 */

static void handle_received_packet(DLPIStream *the_stream, mblk_t *mp, uint16 packet_type, int32 dest_addr_type)
{
        // Find address and header length
        uint32 addr_len;
        uint32 header_len;
        switch (packet_type) {
                case kPkt8022SAP:
                        addr_len = kEnetAndSAPAddressLength;
                        header_len = kEnetPacketHeaderLength + k8022BasicHeaderLength;
                        break;
                case kPkt8022SNAP:
                        addr_len = kEnetAndSAPAddressLength + k8022SNAPLength;
                        header_len = kEnetPacketHeaderLength + k8022SNAPHeaderLength;
                        break;
                default:        // DIX and IPX
                        addr_len = kEnetAndSAPAddressLength;
                        header_len = kEnetPacketHeaderLength;
                        break;
        }

        // In Fast Path mode, don't send DL_UNITDATA_IND messages for unicast packets
        if ((the_stream->flags & kFastPathMode) && dest_addr_type == keaStandardAddress) {
                mp->b_rptr += header_len;
                num_rx_fastpath++;
                putq(the_stream->rdq, mp);
                return;
        }

        // Allocate the dl_unitdata_ind_t message
        mblk_t *nmp;
        if ((nmp = allocb(sizeof(dl_unitdata_ind_t) + 2*addr_len, BPRI_HI)) == NULL) {
                freemsg(mp);
                num_rx_no_unitdata_mem++;
                return;
        }

        // Set message type
        nmp->b_datap->db_type = M_PROTO;
        dl_unitdata_ind_t *ind = (dl_unitdata_ind_t*)(void *)nmp->b_rptr;
        ind->dl_primitive = DL_UNITDATA_IND;
        nmp->b_wptr += (sizeof(dl_unitdata_ind_t) + 2*addr_len);

        // Link M_DATA block
        nmp->b_cont = mp;

        // Set address fields
        ind->dl_dest_addr_length = addr_len;
        ind->dl_dest_addr_offset = sizeof(dl_unitdata_ind_t);
        ind->dl_src_addr_length = addr_len;
        ind->dl_src_addr_offset = sizeof(dl_unitdata_ind_t) + addr_len;

        // Set address type
        ind->dl_group_address = dest_addr_type;

        // Set address fields
        T8022FullPacketHeader *packetHeader = (T8022FullPacketHeader *)(void *)mp->b_rptr;
        T8022AddressStruct *destAddr = ((T8022AddressStruct*)(nmp->b_rptr + ind->dl_dest_addr_offset));
        T8022AddressStruct *srcAddr = ((T8022AddressStruct*)(nmp->b_rptr + ind->dl_src_addr_offset));

        OTCopy48BitAddress(packetHeader->fEnetPart.fDestAddr, destAddr->fHWAddr);
        OTCopy48BitAddress(packetHeader->fEnetPart.fSourceAddr, srcAddr->fHWAddr);

        destAddr->fSAP = packetHeader->f8022Part.fDSAP;
        srcAddr->fSAP = packetHeader->f8022Part.fSSAP;

        if (packet_type == kPkt8022SNAP) {
                OTCopy8022SNAP(packetHeader->f8022Part.fSNAP, destAddr->fSNAP);
                OTCopy8022SNAP(packetHeader->f8022Part.fSNAP, srcAddr->fSNAP);
        }

        // "Hide" the ethernet and protocol header(s)
        mp->b_rptr += header_len;

        // Pass message up the stream
        num_unitdata_ind++;
        putq(the_stream->rdq, nmp);
        return;
}


/*
 *  Packet received, distribute it to the streams that want it
 */

void ether_packet_received(mblk_t *mp)
{
        // Extract address and types
        EnetPacketHeader *pkt = (EnetPacketHeader *)(void *)mp->b_rptr;
        T8022FullPacketHeader *fullpkt = (T8022FullPacketHeader *)pkt;
        uint16 sourceSAP, destSAP;
        destSAP = fullpkt->fEnetPart.fProto;
        if (destSAP >= kMinDIXSAP) {
                // Classic ethernet
                sourceSAP = destSAP;
        } else {
                destSAP = fullpkt->f8022Part.fDSAP;
                sourceSAP = fullpkt->f8022Part.fSSAP;
        }
        uint16 packetType = classify_packet_type(sourceSAP, destSAP);
        int32 destAddressType = get_address_type(pkt->fDestAddr);

        // Look which streams want it
        DLPIStream *the_stream, *found_stream = NULL;
        uint16 found_packetType = 0;
        int32 found_destAddressType = 0;
        for (the_stream = dlpi_stream_list; the_stream != NULL; the_stream = mi_next_ptr(the_stream)) {

                // Don't send to unbound streams
                if (the_stream->dlpi_state == DL_UNBOUND)
                        continue;

                // Does this stream want all 802.2 packets?
                if ((the_stream->flags & kAcceptAll8022Packets) && (destSAP <= 0xff))
                        goto type_found;

                // No, check SAP/SNAP
                if (destSAP == the_stream->dlsap) {
                        if (the_stream->flags & kSnapStream) {
                                // Check SNAPs if necessary
                                uint8 sum = fullpkt->f8022Part.fSNAP[0] ^ the_stream->snap[0];
                                sum |= fullpkt->f8022Part.fSNAP[1] ^ the_stream->snap[1];
                                sum |= fullpkt->f8022Part.fSNAP[2] ^ the_stream->snap[2];
                                sum |= fullpkt->f8022Part.fSNAP[3] ^ the_stream->snap[3];
                                sum |= fullpkt->f8022Part.fSNAP[4] ^ the_stream->snap[4];
                                if (sum == 0)
                                        goto type_found;
                        } else {
                                // No SNAP, found a match since saps match 
                                goto type_found;
                        }
                } else {
                        // Check for an 802.3 Group/Global (odd) 
                        if (((packetType == kPkt8022SAP) || (packetType == kPkt8022SNAP)) && (destSAP & 1) && the_stream->TestGroupSAP(destSAP))
                                goto type_found;
                }

                // No stream for this SAP/SNAP found
                continue;

type_found:
                // If it's a multicast packet, it must be in the stream's multicast list
                if ((destAddressType == keaMulticast) && (the_stream->flags & kAcceptMulticasts) && (!the_stream->IsMulticastRegistered(pkt->fDestAddr)))
                        continue;

                // Send packet to stream
                // found_stream keeps a pointer to the previously found stream, so that only the last
                // stream gets the original message, the other ones get duplicates
                if (found_stream)
                        handle_received_packet(found_stream, dupmsg(mp), found_packetType, found_destAddressType);
                found_stream = the_stream;
                found_packetType = packetType;
                found_destAddressType = destAddressType;
        }

        // Send original message to last found stream
        if (found_stream)
                handle_received_packet(found_stream, mp, found_packetType, found_destAddressType);
        else {
                freemsg(mp);    // Nobody wants it *snief*
                num_rx_dropped++;
        }
}

void ether_dispatch_packet(uint32 p, uint32 size)
{
        // Wrap packet in message block
        num_rx_packets++;
        mblk_t *mp;
        if ((mp = allocb(size, 0)) != NULL) {
                D(bug(" packet data at %p\n", (void *)mp->b_rptr));
                Mac2Host_memcpy(mp->b_rptr, p, size);
                mp->b_wptr += size;
                ether_packet_received(mp);
        } else {
                D(bug("WARNING: Cannot allocate mblk for received packet\n"));
                num_rx_no_mem++;
        }
}


/*
 *  Build and send an error acknowledge
 */

static void DLPI_error_ack(DLPIStream *the_stream, queue_t *q, mblk_t *ack_mp, uint32 prim, uint32 err, uint32 uerr)
{
        D(bug("  DLPI_error_ack(%p,%p) prim %d, err %d, uerr %d\n", the_stream, ack_mp, prim, err, uerr));
        num_error_acks++;

        if (ack_mp != NULL)
                freemsg(ack_mp);
        if ((ack_mp = allocb(sizeof(dl_error_ack_t), BPRI_HI)) == NULL)
                return;

        ack_mp->b_datap->db_type = M_PCPROTO;
        dl_error_ack_t *errp = (dl_error_ack_t *)(void *)ack_mp->b_wptr;
        errp->dl_primitive = DL_ERROR_ACK;
        errp->dl_error_primitive = prim;
        errp->dl_errno = err;
        errp->dl_unix_errno = uerr;
        ack_mp->b_wptr += sizeof(dl_error_ack_t);
        qreply(q, ack_mp);
}


/*
 *  Build and send an OK acknowledge
 */

static void DLPI_ok_ack(DLPIStream *the_stream, queue_t *q, mblk_t *ack_mp, uint32 prim)
{
        if (ack_mp->b_datap->db_ref != 1) {
                // Message already in use, create a new one
                freemsg(ack_mp);
                if ((ack_mp = allocb(sizeof(dl_error_ack_t), BPRI_HI)) == NULL)
                        return;
        } else {
                // Message free
                if (ack_mp->b_cont != NULL) {
                        freemsg(ack_mp->b_cont);
                        ack_mp->b_cont = NULL;
                }
        }

        ack_mp->b_datap->db_type = M_PCPROTO;
        dl_ok_ack_t *ackp = (dl_ok_ack_t *)(void *)ack_mp->b_rptr;
        ackp->dl_primitive = DL_OK_ACK;
        ackp->dl_correct_primitive = prim;
        ack_mp->b_wptr = ack_mp->b_rptr + sizeof(dl_ok_ack_t);
        qreply(q, ack_mp);
}


/*
 *  Handle DL_INFO_REQ (report general information)
 */

static void DLPI_info(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        D(bug("  DLPI_info(%p)\n", the_stream));
        uint32 saplen = 0;
        uint32 addrlen = kEnetPhysicalAddressLength;
        uint32 bcastlen = kEnetPhysicalAddressLength;
        uint32 hdrlen = kEnetPacketHeaderLength;

        // Calculate header length
        if (the_stream->dlpi_state != DL_UNBOUND) {
                saplen = (the_stream->flags & kSnapStream) ? k8022DLSAPLength+k8022SNAPLength : k8022DLSAPLength;
                if (the_stream->dlsap == kSNAPSAP)      
                        hdrlen = kEnetPacketHeaderLength + k8022SNAPHeaderLength;       // SNAP address 
                else if ((the_stream->dlsap <= kMax8022SAP) || (the_stream->dlsap == kIPXSAP))
                        hdrlen = kEnetPacketHeaderLength + k8022BasicHeaderLength;      // SAP or IPX 
                else                                                            
                        hdrlen = kEnetPacketHeaderLength;                                                       // Basic Ethernet
        }

        // Allocate message block for reply
        mblk_t *ack_mp;
        if ((ack_mp = allocb(sizeof(dl_info_ack_t) + addrlen + saplen + bcastlen, BPRI_LO)) == NULL) {
                DLPI_error_ack(the_stream, q, mp, DL_INFO_REQ, DL_SYSERR, MAC_ENOMEM);
                return;
        }

        // Set up message type
        ack_mp->b_datap->db_type = M_PCPROTO;
        dl_info_ack_t *ackp = (dl_info_ack_t *)(void *)ack_mp->b_rptr;
        ackp->dl_primitive = DL_INFO_ACK;

        // Info/version fields
        ackp->dl_service_mode = DL_CLDLS;
        ackp->dl_provider_style = DL_STYLE1;
        ackp->dl_version = DL_VERSION_2;
        ackp->dl_current_state = the_stream->dlpi_state;
        ackp->dl_mac_type = the_stream->framing_8022 ? DL_CSMACD : DL_ETHER;
        ackp->dl_reserved = 0;
        ackp->dl_qos_length = 0;
        ackp->dl_qos_offset = (uint32)DL_UNKNOWN;
        ackp->dl_qos_range_length = 0;
        ackp->dl_qos_range_offset = (uint32)DL_UNKNOWN;
        ackp->dl_growth = 0;
        ackp->dl_min_sdu = 1;
        ackp->dl_max_sdu = kEnetTSDU - hdrlen;

        // Address fields
        ackp->dl_sap_length = -saplen;          // Negative to indicate sap follows physical address
        ackp->dl_addr_length = addrlen + saplen;
        ackp->dl_addr_offset = sizeof(dl_info_ack_t);
        T8022AddressStruct *boundAddr = ((T8022AddressStruct *)(ack_mp->b_rptr + ackp->dl_addr_offset));
        OTCopy48BitAddress(hardware_address, boundAddr->fHWAddr);
        if (saplen) {
                boundAddr->fSAP = the_stream->dlsap;
                if (the_stream->flags & kSnapStream) 
                        OTCopy8022SNAP(the_stream->snap, boundAddr->fSNAP);
        }
        ackp->dl_brdcst_addr_length = bcastlen;
        ackp->dl_brdcst_addr_offset = sizeof(dl_info_ack_t) + addrlen + saplen;
        OTSet48BitBroadcastAddress(ack_mp->b_rptr + ackp->dl_brdcst_addr_offset);       

        // Advance write pointer
        ack_mp->b_wptr += sizeof(dl_info_ack_t) + addrlen + saplen + bcastlen;

        // Free request
        freemsg(mp);

        // Send reply
        qreply(q, ack_mp);
        return;
}


/*
 *  Handle DL_PHYS_ADDR_REQ (report physical address)
 */

static void DLPI_phys_addr(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        D(bug("  DLPI_phys_addr(%p,%p)\n", the_stream, mp));
        dl_phys_addr_req_t *req = (dl_phys_addr_req_t *)(void *)mp->b_rptr;

        // Allocate message block for reply
        mblk_t *ack_mp;
        if ((ack_mp = allocb(sizeof(dl_phys_addr_ack_t) + kEnetPhysicalAddressLength, BPRI_HI)) == NULL) {
                DLPI_error_ack(the_stream, q, mp, DL_PHYS_ADDR_REQ, DL_SYSERR, MAC_ENOMEM);
                return;
        }

        // Set up message type
        ack_mp->b_datap->db_type = M_PCPROTO;
        dl_phys_addr_ack_t *ackp = (dl_phys_addr_ack_t *)(void *)ack_mp->b_wptr;
        ackp->dl_primitive = DL_PHYS_ADDR_ACK;

        // Fill in address
        ackp->dl_addr_length = kEnetPhysicalAddressLength;
        ackp->dl_addr_offset = sizeof(dl_phys_addr_ack_t);
        ack_mp->b_wptr += sizeof(dl_phys_addr_ack_t) + kEnetPhysicalAddressLength;
        if (req->dl_addr_type == DL_CURR_PHYS_ADDR || req->dl_addr_type == DL_FACT_PHYS_ADDR)
                OTCopy48BitAddress(hardware_address, ack_mp->b_rptr + ackp->dl_addr_offset);
        else {
                DLPI_error_ack(the_stream, q, mp, DL_PHYS_ADDR_REQ, DL_BADPRIM, 0);
                return;
        }

        // Free request
        freemsg(mp);

        // Send reply
        qreply(q, ack_mp);
        return;
}


/*
 *  Handle DL_BIND_REQ (bind a stream)
 */

static void DLPI_bind(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        dl_bind_req_t *req = (dl_bind_req_t *)(void *)mp->b_rptr;
        uint32 sap = req->dl_sap;
        D(bug("  DLPI_bind(%p,%p) SAP %04x\n", the_stream, mp, sap));

        // Stream must be unbound
        if (the_stream->dlpi_state != DL_UNBOUND) {
                DLPI_error_ack(the_stream, q, mp, DL_BIND_REQ, DL_OUTSTATE, 0);
                return;
        }

        // We only support connectionless data link services
        if (req->dl_service_mode != DL_CLDLS || req->dl_max_conind != 0) {
                DLPI_error_ack(the_stream, q, mp, DL_BIND_REQ, DL_UNSUPPORTED, 0);
                return;
        }

        // Don't bind to 802.2 group saps, can't check 802.2 global sap (0xFF)
        // because it looks like IPX
        if ((sap <= kMax8022SAP) && (sap & 1)) {
                DLPI_error_ack(the_stream, q, mp, DL_BIND_REQ, DL_BADADDR, 0);
                return;
        }

        if (classify_packet_type(sap, sap) == kPktUnknown) {
                DLPI_error_ack(the_stream, q, mp, DL_BIND_REQ, DL_BADADDR, 0);
                return;
        }

        // Allocate message block for reply
        mblk_t *ack_mp;
        if ((ack_mp = allocb(sizeof(dl_bind_ack_t) + kEnetAndSAPAddressLength, BPRI_HI)) == NULL) {
                DLPI_error_ack(the_stream, q, mp, DL_BIND_REQ, DL_SYSERR, MAC_ENOMEM);
                return;
        }

        // Set up message type
        ack_mp->b_datap->db_type = M_PCPROTO;
        dl_bind_ack_t *ackp = (dl_bind_ack_t *)(void *)ack_mp->b_rptr;
        ackp->dl_primitive = DL_BIND_ACK;

        // Fill in other fields
        ackp->dl_sap = sap;
        ackp->dl_addr_length = kEnetAndSAPAddressLength;                
        ackp->dl_addr_offset = sizeof(dl_bind_ack_t);
        ackp->dl_max_conind = 0;
        ackp->dl_xidtest_flg = 0;

        T8022AddressStruct *addrInfo = (T8022AddressStruct *)(ack_mp->b_rptr + sizeof(dl_bind_ack_t));
        OTCopy48BitAddress(hardware_address, addrInfo->fHWAddr);
        addrInfo->fSAP = sap;

        // Must move b_wptr past the address info data
        ack_mp->b_wptr = ack_mp->b_rptr + sizeof(dl_bind_ack_t) + kEnetAndSAPAddressLength;

        // Set group SAP if necessary
        the_stream->ClearAllGroupSAPs();
        if (sap <= kMax8022SAP)
                the_stream->SetGroupSAP(k8022GlobalSAP);        

        // The stream is now bound and idle
        the_stream->dlpi_state = DL_IDLE;
        the_stream->dlsap = sap;
        the_stream->flags &= ~kSnapStream;

        // Free request
        freemsg(mp);

        // Send reply
        qreply(q, ack_mp);
        return;
}


/*
 *  Handle DL_UNBIND_REQ (unbind a stream)
 */

static void DLPI_unbind(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        D(bug("  DLPI_unbind(%p,%p)\n", the_stream, mp));

        // Stream must be bound and idle
        if (the_stream->dlpi_state != DL_IDLE) {
                DLPI_error_ack(the_stream, q, mp, DL_UNBIND_REQ, DL_OUTSTATE, 0);
                return;
        }

        // Stream is now unbound
        the_stream->dlpi_state = DL_UNBOUND;
        the_stream->dlsap = 0;                          

        // Flush all pending outbound messages
        flushq(q, FLUSHDATA);

        // Flush all inbound messages pending on the stream
        flushq(RD(q), FLUSHDATA);
        putnextctl1(RD(q), M_FLUSH, FLUSHRW);

        // Send reply
        DLPI_ok_ack(the_stream, q, mp, DL_UNBIND_REQ);
        return;
}


/*
 *  Handle DL_SUBS_BIND_REQ (register 802.2 SAP group addresses and SNAPs)
 */

static void DLPI_subs_bind(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        dl_subs_bind_req_t *req = (dl_subs_bind_req_t *)(void *)mp->b_rptr;
        uint8 *sap = ((uint8 *)req) + req->dl_subs_sap_offset;
        int32 length = req->dl_subs_sap_length;
        uint16 theSap = ntohs(*((uint16 *)sap));
        int32 error = 0;
        D(bug("  DLPI_subs_bind(%p,%p) SAP %02x%02x%02x%02x%02x\n", the_stream, mp, sap[0], sap[1], sap[2], sap[3], sap[4]));

        // Stream must be idle
        if (the_stream->dlpi_state != DL_IDLE) {
                DLPI_error_ack(the_stream, q, mp, DL_SUBS_BIND_REQ, DL_OUTSTATE, 0);
                return;
        }

        // Check if address is valid
        switch (req->dl_subs_bind_class) {
                case DL_PEER_BIND:                      // Bind a group address
                        if (the_stream->dlsap <= kMax8022SAP) {
                                if ((theSap & 1) && (length == sizeof(theSap)))
                                        the_stream->SetGroupSAP(theSap);
                                else
                                        if (theSap == 0x0000)   // special case to receive all 802.2 packets
                                                the_stream->flags |= kAcceptAll8022Packets;
                                        else
                                                error = DL_BADADDR;
                        } else
                                error = DL_UNSUPPORTED;
                        break;  

                case DL_HIERARCHICAL_BIND:      // Bind an additional SNAP
                        if (the_stream->dlsap == kSNAPSAP) {
                                if (the_stream->flags & kSnapStream) 
                                        error = DL_TOOMANY;     // only one SNAP binding allowed 
                                else {
                                        OTCopy8022SNAP(sap, the_stream->snap);
                                        the_stream->flags |= kSnapStream;
                                }
                        } else
                                error = DL_BADADDR;
                        break;

                default:
                        error = DL_UNSUPPORTED;
                        break;
        }
        if (error) {
                DLPI_error_ack(the_stream, q, mp, DL_SUBS_BIND_REQ, error, 0);
                return;
        }

        // Allocate message block for reply
        mblk_t *ack_mp;
        if ((ack_mp = allocb(sizeof(dl_subs_bind_ack_t) + length, BPRI_HI)) == NULL) {
                DLPI_error_ack(the_stream, q, mp, DL_SUBS_BIND_REQ, DL_SYSERR, MAC_ENOMEM);
                return;
        }

        // Set up message type
        ack_mp->b_datap->db_type = M_PCPROTO;
        dl_subs_bind_ack_t *ackp = (dl_subs_bind_ack_t *)(void *)ack_mp->b_wptr;
        memset(ackp, 0, sizeof(dl_subs_bind_ack_t) + length);
        ackp->dl_primitive = DL_SUBS_BIND_ACK;

        // Fill in other fields
        ackp->dl_subs_sap_length = length;
        ackp->dl_subs_sap_offset = length ? sizeof(dl_subs_bind_ack_t) : 0;
        ack_mp->b_wptr += sizeof(dl_subs_bind_ack_t);
        if (length)
                memcpy(ack_mp->b_wptr, sap, length);
        ack_mp->b_wptr += length;

        // Free request
        freemsg(mp);

        // Send reply
        qreply(q, ack_mp);
        return;
}


/*
 *  Handle DL_SUBS_UNBIND_REQ (unregister 802.2 SAP group addresses and snaps)
 */

static void DLPI_subs_unbind(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        dl_subs_unbind_req_t *req = (dl_subs_unbind_req_t *)(void *)mp->b_rptr;
        uint8 *sap = ((uint8 *)req) + req->dl_subs_sap_offset;
        int32 length = req->dl_subs_sap_length;
        int32 error = 0;
        D(bug("  DLPI_subs_unbind(%p,%p) SAP %02x%02x%02x%02x%02x\n", the_stream, mp, sap[0], sap[1], sap[2], sap[3], sap[4]));

        // Stream must be idle
        if (the_stream->dlpi_state != DL_IDLE) {
                DLPI_error_ack(the_stream, q, mp, DL_SUBS_UNBIND_REQ, DL_OUTSTATE, 0);
                return;
        }

        // Check if we are unbinding from an address we are bound to
        if (length == k8022SAPLength) {
                if ((*sap & 1) && (*sap != kIPXSAP)) {
                        if (the_stream->dlsap <= kMax8022SAP)
                                the_stream->ClearGroupSAP(*sap);
                        else
                                error = DL_UNSUPPORTED;
                } else
                        error = DL_BADADDR;
        } else if (length == k8022SNAPLength) {
                if (the_stream->dlsap == kSNAPSAP) {
                        if (the_stream->flags & kSnapStream) {
                                if (memcmp(the_stream->snap, sap, length) != 0) 
                                        error = DL_BADADDR;
                        } else
                                error = DL_BADADDR;
                } else
                        error = DL_UNSUPPORTED;
        }
        if (error) {
                DLPI_error_ack(the_stream, q, mp, DL_SUBS_UNBIND_REQ, error, 0);
                return;
        }

        // Stream is no longer bound to SNAP
        the_stream->flags &= ~kSnapStream;

        // Send reply
        DLPI_ok_ack(the_stream, q, mp, DL_SUBS_UNBIND_REQ);
        return;
}


/*
 *  Handles DL_ENABMULTI_REQ (enable multicast address)
 */

static void DLPI_enable_multi(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        dl_enabmulti_req_t*     req = (dl_enabmulti_req_t*)(void *)mp->b_rptr;
        uint8 *reqaddr = (uint8 *)(mp->b_rptr + req->dl_addr_offset);
        D(bug("  DLPI_enable_multi(%p,%p) addr %02x%02x%02x%02x%02x%02x\n", the_stream, mp, reqaddr[0], reqaddr[1], reqaddr[2], reqaddr[3], reqaddr[4], reqaddr[5]));

        // Address must be a multicast address
        if (get_address_type(reqaddr) != keaMulticast) {
                DLPI_error_ack(the_stream, q, mp, DL_ENABMULTI_REQ, DL_BADADDR, 0);
                return;
        }

        // Address already in multicast list?
        if (the_stream->IsMulticastRegistered(reqaddr)) {
                DLPI_error_ack(the_stream, q, mp, DL_ENABMULTI_REQ, DL_BADADDR, 0);
                return;
        }

        // Tell add-on to enable multicast address
        AO_enable_multicast(Host2MacAddr((uint8 *)reqaddr));

        // Add new address to multicast list
        uint8 *addr = Mac2HostAddr(Mac_sysalloc(kEnetPhysicalAddressLength));
        OTCopy48BitAddress(reqaddr, addr);
        the_stream->AddMulticast(addr);

        // On receive now check multicast packets
        the_stream->flags |= kAcceptMulticasts;

        // Send reply
        DLPI_ok_ack(the_stream, q, mp, DL_ENABMULTI_REQ);
        return;
}


/*
 *  Handles DL_DISABMULTI_REQ (disable multicast address)
 */

static void DLPI_disable_multi(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        dl_disabmulti_req_t *req = (dl_disabmulti_req_t*)(void *)mp->b_rptr;
        uint8 *reqaddr = (uint8 *)(mp->b_rptr + req->dl_addr_offset);
        D(bug("  DLPI_disable_multi(%p,%p) addr %02x%02x%02x%02x%02x%02x\n", the_stream, mp, reqaddr[0], reqaddr[1], reqaddr[2], reqaddr[3], reqaddr[4], reqaddr[5]));

        // Address must be a multicast address
        if (get_address_type(reqaddr) != keaMulticast) {
                DLPI_error_ack(the_stream, q, mp, DL_DISABMULTI_REQ, DL_BADADDR, 0);
                return;
        }

        // Find address in multicast list
        uint8 *addr = the_stream->IsMulticastRegistered(reqaddr);
        if (addr == NULL) {
                DLPI_error_ack(the_stream, q, mp, DL_DISABMULTI_REQ, DL_BADADDR, 0);
                return;
        }

        // Found, then remove
        the_stream->RemoveMulticast(addr);
        Mac_sysfree(Host2MacAddr(addr));

        // Tell add-on to disable multicast address
        AO_disable_multicast(Host2MacAddr((uint8 *)reqaddr));
        
        // No longer check multicast packets if no multicast addresses are registered
        if (the_stream->multicast_list == NULL)
                the_stream->flags &= ~kAcceptMulticasts;
                        
        // Send reply
        DLPI_ok_ack(the_stream, q, mp, DL_DISABMULTI_REQ);
        return;
}


/*
 *  Handle DL_UNITDATA_REQ (transmit packet)
 */

static void DLPI_unit_data(DLPIStream *the_stream, queue_t *q, mblk_t *mp)
{
        D(bug("  DLPI_unit_data(%p,%p)\n", the_stream, mp));
        dl_unitdata_req_t *req = (dl_unitdata_req_t *)(void *)mp->b_rptr;

        // Stream must be idle
        if (the_stream->dlpi_state != DL_IDLE) {

                // Not idle, send error response
                dl_uderror_ind_t *errp;
                mblk_t *bp;

                int i = sizeof(dl_uderror_ind_t) + req->dl_dest_addr_length;
                if ((bp = allocb(i, BPRI_HI)) == NULL) {
                        freemsg(mp);
                        return;
                }
                bp->b_datap->db_type = M_PROTO;
                errp = (dl_uderror_ind_t *)(void *)bp->b_wptr;
                errp->dl_primitive = DL_UDERROR_IND;
                errp->dl_errno = DL_OUTSTATE;
                errp->dl_unix_errno = 0;
                errp->dl_dest_addr_length = req->dl_dest_addr_length;
                errp->dl_dest_addr_offset = sizeof(dl_uderror_ind_t);
                bp->b_wptr += sizeof(dl_uderror_ind_t);
                memcpy((uint8 *)bp->b_wptr, ((uint8 *)req) + req->dl_dest_addr_offset, req->dl_dest_addr_length);
                bp->b_wptr += req->dl_dest_addr_length;
                qreply(q, bp);

                freemsg(mp);
                return;
        }

        // Build packet header and transmit packet
        if ((mp = build_tx_packet_header(the_stream, mp, false)) != NULL)
                transmit_packet(mp);
}


/*
 *  Ethernet packet allocator
 */

#if SIZEOF_VOID_P != 4 || REAL_ADDRESSING == 0
static uint32 ether_packet = 0;                 // Ethernet packet (cached allocation)
static uint32 n_ether_packets = 0;              // Number of ethernet packets allocated so far (should be at most 1)

EthernetPacket::EthernetPacket()
{
        ++n_ether_packets;
        if (ether_packet && n_ether_packets == 1)
                packet = ether_packet;
        else {
                packet = Mac_sysalloc(1516);
                assert(packet != 0);
                Mac_memset(packet, 0, 1516);
                if (ether_packet == 0)
                        ether_packet = packet;
        }
}

EthernetPacket::~EthernetPacket()
{
        --n_ether_packets;
        if (packet != ether_packet)
                Mac_sysfree(packet);
        if (n_ether_packets > 0) {
                bug("WARNING: Nested allocation of ethernet packets!\n");
        }
}
#endif
Revision:	1.8
Committed:	2005-07-03T08:23:42Z (18 years, 10 months ago) by gbeauche
Branch:	MAIN
Changes since 1.7:	+56 -6 lines
Log Message:	First round of patches to increase ethernet code portability.