- char *current_ptr;
- struct dlil_demux_desc *desc;
- struct en_desc *ed;
- struct en_desc *last;
- u_long *bitmask;
- u_long *proto_id;
- u_long i;
- short total_length;
- u_long block_count;
- u_long *tmp;
-
-
- TAILQ_FOREACH(desc, desc_head, next) {
- switch (desc->type) {
- /* These types are supported */
- /* Top three are preferred */
- case DLIL_DESC_ETYPE2:
- if (desc->variants.native_type_length != 2)
- return EINVAL;
- break;
-
- case DLIL_DESC_SAP:
- if (desc->variants.native_type_length != 3)
- return EINVAL;
- break;
-
- case DLIL_DESC_SNAP:
- if (desc->variants.native_type_length != 5)
- return EINVAL;
- break;
-
- case DLIL_DESC_802_2:
- case DLIL_DESC_802_2_SNAP:
- break;
-
- case DLIL_DESC_RAW:
- if (desc->variants.bitmask.proto_id_length == 0)
- break;
- /* else fall through, bitmask variant not supported */
-
- default:
- ether_del_proto(proto, dl_tag);
- return EINVAL;
- }
-
- restart:
- ed = ether_desc_blk[proto->ifp->family_cookie].block_ptr;
-
- /* Find a free entry */
- for (i = 0; i < ether_desc_blk[proto->ifp->family_cookie].n_count; i++) {
- if (ed[i].type == 0) {
- break;
- }
- }
-
- if (i >= ether_desc_blk[proto->ifp->family_cookie].n_count) {
- u_long new_count = ETHER_DESC_BLK_SIZE +
- ether_desc_blk[proto->ifp->family_cookie].n_count;
- tmp = _MALLOC((new_count * (sizeof(*ed))), M_IFADDR, M_WAITOK);
- if (tmp == 0) {
- /*
- * Remove any previous descriptors set in the call.
- */
- ether_del_proto(proto, dl_tag);
- return ENOMEM;
- }
-
- bzero(tmp, new_count * sizeof(*ed));
- bcopy(ether_desc_blk[proto->ifp->family_cookie].block_ptr,
- tmp, ether_desc_blk[proto->ifp->family_cookie].n_count * sizeof(*ed));
- FREE(ether_desc_blk[proto->ifp->family_cookie].block_ptr, M_IFADDR);
- ether_desc_blk[proto->ifp->family_cookie].n_count = new_count;
- ether_desc_blk[proto->ifp->family_cookie].block_ptr = (struct en_desc*)tmp;
- }
-
- /* Bump n_max_used if appropriate */
- if (i + 1 > ether_desc_blk[proto->ifp->family_cookie].n_max_used) {
- ether_desc_blk[proto->ifp->family_cookie].n_max_used = i + 1;
- }
-
- ed[i].proto = proto;
- ed[i].data[0] = 0;
- ed[i].data[1] = 0;
-
- switch (desc->type) {
- case DLIL_DESC_RAW:
- /* 2 byte ethernet raw protocol type is at native_type */
- /* protocol is not in network byte order */
- ed[i].type = DLIL_DESC_ETYPE2;
- ed[i].data[0] = htons(*(u_int16_t*)desc->native_type);
- break;
-
- case DLIL_DESC_ETYPE2:
- /* 2 byte ethernet raw protocol type is at native_type */
- /* prtocol must be in network byte order */
- ed[i].type = DLIL_DESC_ETYPE2;
- ed[i].data[0] = *(u_int16_t*)desc->native_type;
- break;
-
- case DLIL_DESC_802_2:
- ed[i].type = DLIL_DESC_SAP;
- ed[i].data[0] = *(u_int32_t*)&desc->variants.desc_802_2;
- ed[i].data[0] &= htonl(0xFFFFFF00);
- break;
-
- case DLIL_DESC_SAP:
- ed[i].type = DLIL_DESC_SAP;
- bcopy(desc->native_type, &ed[i].data[0], 3);
- break;
-
- case DLIL_DESC_802_2_SNAP:
- ed[i].type = DLIL_DESC_SNAP;
- desc->variants.desc_802_2_SNAP.protocol_type =
- htons(desc->variants.desc_802_2_SNAP.protocol_type);
- bcopy(&desc->variants.desc_802_2_SNAP, &ed[i].data[0], 8);
- ed[i].data[0] &= htonl(0x000000FF);
- desc->variants.desc_802_2_SNAP.protocol_type =
- ntohs(desc->variants.desc_802_2_SNAP.protocol_type);
- break;
-
- case DLIL_DESC_SNAP: {
- u_int8_t* pDest = ((u_int8_t*)&ed[i].data[0]) + 3;
- ed[i].type = DLIL_DESC_SNAP;
- bcopy(desc->native_type, pDest, 5);
- }
- break;
- }
- }
-
- return 0;
+ struct en_desc *ed;
+ struct ether_desc_blk_str *desc_blk = (struct ether_desc_blk_str *)ifp->family_cookie;
+ u_int32_t i;
+
+ switch (demux->type) {
+ /* These types are supported */
+ /* Top three are preferred */
+ case DLIL_DESC_ETYPE2:
+ if (demux->datalen != 2) {
+ return EINVAL;
+ }
+ break;
+
+ case DLIL_DESC_SAP:
+ if (demux->datalen != 3) {
+ return EINVAL;
+ }
+ break;
+
+ case DLIL_DESC_SNAP:
+ if (demux->datalen != 5) {
+ return EINVAL;
+ }
+ break;
+
+ default:
+ return ENOTSUP;
+ }
+
+ // Verify a matching descriptor does not exist.
+ if (desc_blk != NULL) {
+ switch (demux->type) {
+ case DLIL_DESC_ETYPE2:
+ for (i = 0; i < desc_blk->n_max_used; i++) {
+ if (desc_blk->block_ptr[i].type == DLIL_DESC_ETYPE2 &&
+ desc_blk->block_ptr[i].data[0] ==
+ *(u_int16_t*)demux->data) {
+ return EADDRINUSE;
+ }
+ }
+ break;
+ case DLIL_DESC_SAP:
+ case DLIL_DESC_SNAP:
+ for (i = 0; i < desc_blk->n_max_used; i++) {
+ if (desc_blk->block_ptr[i].type == demux->type &&
+ bcmp(desc_blk->block_ptr[i].data, demux->data,
+ demux->datalen) == 0) {
+ return EADDRINUSE;
+ }
+ }
+ break;
+ }
+ }
+
+ // Check for case where all of the descriptor blocks are in use
+ if (desc_blk == NULL || desc_blk->n_used == desc_blk->n_count) {
+ struct ether_desc_blk_str *tmp;
+ u_long new_count = ETHER_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 en_desc) + ETHER_DESC_HEADER_SIZE;
+ i = desc_blk->n_used;
+ }
+
+ new_size = new_count * sizeof(struct en_desc) + ETHER_DESC_HEADER_SIZE;
+
+ tmp = _MALLOC(new_size, M_IFADDR, M_WAITOK);
+ if (tmp == 0) {
+ /*
+ * Remove any previous descriptors set in the call.
+ */
+ 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;
+ ifp->family_cookie = (u_long)desc_blk;
+ desc_blk->n_count = new_count;
+ }
+ else {
+ /* Find a free entry */
+ for (i = 0; i < desc_blk->n_count; i++) {
+ if (desc_blk->block_ptr[i].type == 0) {
+ break;
+ }
+ }
+ }
+
+ /* Bump n_max_used if appropriate */
+ 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;
+ ed->data[0] = 0;
+ ed->data[1] = 0;
+
+ switch (demux->type) {
+ case DLIL_DESC_ETYPE2:
+ /* 2 byte ethernet raw protocol type is at native_type */
+ /* prtocol must be in network byte order */
+ ed->type = DLIL_DESC_ETYPE2;
+ ed->data[0] = *(u_int16_t*)demux->data;
+ break;
+
+ case DLIL_DESC_SAP:
+ ed->type = DLIL_DESC_SAP;
+ bcopy(demux->data, &ed->data[0], 3);
+ break;
+
+ case DLIL_DESC_SNAP: {
+ u_int8_t* pDest = ((u_int8_t*)&ed->data[0]) + 3;
+ ed->type = DLIL_DESC_SNAP;
+ bcopy(demux->data, pDest, 5);
+ }
+ break;
+ }
+
+ desc_blk->n_used++;
+
+ return 0;
+}
+
+int
+ether_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 = ether_add_proto_internal(ifp, protocol, &demux_list[i]);
+ if (error) {
+ ether_del_proto(ifp, protocol);
+ break;
+ }
+ }
+
+ return error;
+}
+
+__private_extern__ int
+ether_add_proto_old(
+ struct ifnet *ifp,
+ u_long protocol_family,
+ struct ddesc_head_str *desc_head)
+{
+ struct dlil_demux_desc *desc;
+ int error = 0;
+
+ TAILQ_FOREACH(desc, desc_head, next) {
+ struct ifnet_demux_desc dmx;
+ int swapped = 0;
+
+ // Convert dlil_demux_desc to ifnet_demux_desc
+ dmx.type = desc->type;
+ dmx.datalen = desc->variants.native_type_length;
+ dmx.data = desc->native_type;
+
+#ifdef DLIL_DESC_RAW
+ if (dmx.type == DLIL_DESC_RAW) {
+ swapped = 1;
+ dmx.type = DLIL_DESC_ETYPE2;
+ dmx.datalen = 2;
+ *(u_int16_t*)dmx.data = htons(*(u_int16_t*)dmx.data);
+ }
+#endif
+
+ error = ether_add_proto_internal(ifp, protocol_family, &dmx);
+ if (swapped) {
+ *(u_int16_t*)dmx.data = ntohs(*(u_int16_t*)dmx.data);
+ swapped = 0;
+ }
+ if (error) {
+ ether_del_proto(ifp, protocol_family);
+ break;
+ }
+ }
+
+ return error;
projects / apple / xnu.git / blobdiff