firewire_if_module.cpp [plain text]
#ifndef INET
#define INET 1
#endif
extern "C"{
#include <sys/param.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/socket.h>
#include <sys/sockio.h>
#include <net/dlil.h>
#include <sys/sysctl.h>
#include <sys/systm.h>
#include <sys/syslog.h>
#include <net/if.h>
#include <net/route.h>
#include <net/if_llc.h>
#include <net/if_dl.h>
#include <net/if_types.h>
#include <netinet/in.h>
#include <sys/socketvar.h>
#include <net/dlil.h>
#include "firewire.h"
#include <if_firewire.h>
}
#include "IOFireWireIP.h"
struct fw_desc {
u_int16_t type;
u_long protocol_family;
u_long data[2];
};
#define FIREWIRE_DESC_BLK_SIZE (10)
struct firewire_desc_blk_str {
u_long n_max_used;
u_long n_count;
u_long n_used;
struct fw_desc block_ptr[1];
};
#define FIREWIRE_DESC_HEADER_SIZE ((size_t)&(((struct firewire_desc_blk_str*)0)->block_ptr[0]))
__private_extern__
int firewire_del_proto(ifnet_t ifp, protocol_family_t protocol_family)
{
IOFWInterface *fwIf = (IOFWInterface*)ifnet_softc(ifp);
struct firewire_desc_blk_str *desc_blk = (struct firewire_desc_blk_str *)fwIf->getFamilyCookie();
u_long current = 0;
int found = 0;
if (desc_blk == NULL)
return 0;
for (current = desc_blk->n_max_used; current > 0; current--)
{
if (desc_blk->block_ptr[current - 1].protocol_family == protocol_family)
{
found = 1;
desc_blk->block_ptr[current - 1].type = 0;
desc_blk->n_used--;
}
}
if (desc_blk->n_used == 0)
{
FREE(fwIf->getFamilyCookie(), M_IFADDR);
fwIf->setFamilyCookie(NULL);
}
else
{
for (; desc_blk->n_max_used > 0 && desc_blk->block_ptr[desc_blk->n_max_used - 1].type == 0; desc_blk->n_max_used--)
;
}
return found;
}
__private_extern__ int
firewire_add_proto_internal(ifnet_t ifp, u_long protocol_family, const struct ifnet_demux_desc *demux)
{
IOFWInterface *fwIf = (IOFWInterface*)ifnet_softc(ifp);
struct firewire_desc_blk_str *desc_blk = (struct firewire_desc_blk_str *)fwIf->getFamilyCookie();
struct fw_desc *ed;
u_long i;
switch (demux->type)
{
case DLIL_DESC_ETYPE2:
if (demux->datalen != 2)
return EINVAL;
break;
default:
return EOPNOTSUPP;
}
if (desc_blk == NULL || desc_blk->n_used == desc_blk->n_count)
{
struct firewire_desc_blk_str *tmp;
u_long new_count = FIREWIRE_DESC_BLK_SIZE;
u_long new_size;
u_long old_size = 0;
i = 0;
if (desc_blk)
{
new_count += desc_blk->n_count;
old_size = desc_blk->n_count * sizeof(struct fw_desc) + FIREWIRE_DESC_HEADER_SIZE;
i = desc_blk->n_used;
}
new_size = new_count * sizeof(struct fw_desc) + FIREWIRE_DESC_HEADER_SIZE;
tmp = (struct firewire_desc_blk_str*)_MALLOC(new_size, M_IFADDR, M_WAITOK);
if (tmp == 0)
return ENOMEM;
bzero(tmp + old_size, new_size - old_size);
if (desc_blk)
{
bcopy(desc_blk, tmp, old_size);
FREE(desc_blk, M_IFADDR);
}
desc_blk = tmp;
fwIf->setFamilyCookie(desc_blk);
desc_blk->n_count = new_count;
}
else
{
for (i = 0; i < desc_blk->n_count; i++)
{
if (desc_blk->block_ptr[i].type == 0)
break;
}
}
if (i + 1 > desc_blk->n_max_used) {
desc_blk->n_max_used = i + 1;
}
ed = &desc_blk->block_ptr[i];
ed->protocol_family = protocol_family;
ed->data[0] = 0;
ed->data[1] = 0;
switch (demux->type) {
case DLIL_DESC_ETYPE2:
ed->type = DLIL_DESC_ETYPE2;
ed->data[0] = *(u_int16_t*)demux->data;
break;
}
desc_blk->n_used++;
return 0;
}
int
firewire_add_proto(ifnet_t ifp, protocol_family_t protocol, const struct ifnet_demux_desc *demux_list, u_int32_t demux_count)
{
int error = 0;
u_int32_t i;
for (i = 0; i < demux_count; i++)
{
error = firewire_add_proto_internal(ifp, protocol, &demux_list[i]);
if (error)
{
firewire_del_proto(ifp, protocol);
break;
}
}
return error;
}
__private_extern__ int firewire_demux(ifnet_t ifp, mbuf_t m, char *frame_header, protocol_family_t *protocol_family)
{
register struct firewire_header *eh = (struct firewire_header *)frame_header;
IOFWInterface *fwIf = (IOFWInterface*)ifnet_softc(ifp);
struct firewire_desc_blk_str *desc_blk = (struct firewire_desc_blk_str *)fwIf->getFamilyCookie();
u_short fw_type = eh->fw_type;
u_int16_t type = DLIL_DESC_ETYPE2;
u_long i = 0;
u_long maxd = desc_blk->n_max_used;
struct fw_desc *ed = desc_blk->block_ptr;
for (i = 0; i < maxd; i++)
{
if ((ed[i].type == type) && (ed[i].data[0] == fw_type))
{
*protocol_family = ed[i].protocol_family;
return 0;
}
}
return ENOENT;
}
extern struct ifnet *lo_ifp;
__private_extern__ int
firewire_frameout(ifnet_t ifp, mbuf_t *m,
const struct sockaddr *ndest, const char *edst, const char *fw_type)
{
register struct firewire_header *fwh;
if ((ifnet_flags(ifp) & IFF_SIMPLEX) &&
(((struct mbuf*)(*m))->m_flags & M_LOOP))
{
if (lo_ifp)
{
if (((struct mbuf*)(*m))->m_flags & M_BCAST)
{
struct mbuf *n = m_copy((struct mbuf*)*m, 0, (int)M_COPYALL);
if (n != NULL)
dlil_output(lo_ifp, PF_INET, n, 0, ndest, 0);
}
else
{
if (bcmp(edst, ifnet_lladdr(ifp), FIREWIRE_ADDR_LEN) == 0)
{
dlil_output(lo_ifp, PF_INET, (struct mbuf*)*m, 0, ndest, 0);
return EJUSTRETURN;
}
}
}
}
M_PREPEND((struct mbuf*)*m, sizeof(struct firewire_header), M_DONTWAIT);
if (*m == 0)
return (EJUSTRETURN);
fwh = mtod((struct mbuf*)*m, struct firewire_header *);
(void)memcpy(&fwh->fw_type, fw_type,sizeof(fwh->fw_type));
memcpy(fwh->fw_dhost, edst, FIREWIRE_ADDR_LEN);
(void)memcpy(fwh->fw_shost, ifnet_lladdr(ifp), sizeof(fwh->fw_shost));
return 0;
}
__private_extern__
int firewire_add_if(ifnet_t ifp)
{
return 0;
}
__private_extern__
int firewire_del_if(IOFWInterface *fwIf)
{
if (fwIf->getFamilyCookie()) {
FREE(fwIf->getFamilyCookie(), M_IFADDR);
return 0;
}
else
return ENOENT;
}
__private_extern__ int
firewire_ifmod_ioctl(ifnet_t ifp, u_long cmd, void *data)
{
int err = EOPNOTSUPP;
return err;
}
__private_extern__ int
firewire_init_if(ifnet_t ifp)
{
return 0;
}