| 1 | /* $NetBSD: if_ieee1394subr.c,v 1.58 2016/10/03 11:06:06 ozaki-r Exp $ */ |
| 2 | |
| 3 | /* |
| 4 | * Copyright (c) 2000 The NetBSD Foundation, Inc. |
| 5 | * All rights reserved. |
| 6 | * |
| 7 | * This code is derived from software contributed to The NetBSD Foundation |
| 8 | * by Atsushi Onoe. |
| 9 | * |
| 10 | * Redistribution and use in source and binary forms, with or without |
| 11 | * modification, are permitted provided that the following conditions |
| 12 | * are met: |
| 13 | * 1. Redistributions of source code must retain the above copyright |
| 14 | * notice, this list of conditions and the following disclaimer. |
| 15 | * 2. Redistributions in binary form must reproduce the above copyright |
| 16 | * notice, this list of conditions and the following disclaimer in the |
| 17 | * documentation and/or other materials provided with the distribution. |
| 18 | * |
| 19 | * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS |
| 20 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
| 21 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
| 22 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS |
| 23 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 29 | * POSSIBILITY OF SUCH DAMAGE. |
| 30 | */ |
| 31 | |
| 32 | #include <sys/cdefs.h> |
| 33 | __KERNEL_RCSID(0, "$NetBSD: if_ieee1394subr.c,v 1.58 2016/10/03 11:06:06 ozaki-r Exp $" ); |
| 34 | |
| 35 | #ifdef _KERNEL_OPT |
| 36 | #include "opt_inet.h" |
| 37 | #endif |
| 38 | |
| 39 | #include <sys/param.h> |
| 40 | #include <sys/systm.h> |
| 41 | #include <sys/bus.h> |
| 42 | #include <sys/device.h> |
| 43 | #include <sys/kernel.h> |
| 44 | #include <sys/mbuf.h> |
| 45 | #include <sys/socket.h> |
| 46 | #include <sys/sockio.h> |
| 47 | #include <sys/select.h> |
| 48 | |
| 49 | #include <net/if.h> |
| 50 | #include <net/if_dl.h> |
| 51 | #include <net/if_ieee1394.h> |
| 52 | #include <net/if_types.h> |
| 53 | #include <net/if_media.h> |
| 54 | #include <net/ethertypes.h> |
| 55 | #include <net/netisr.h> |
| 56 | #include <net/route.h> |
| 57 | |
| 58 | #include <net/bpf.h> |
| 59 | |
| 60 | #ifdef INET |
| 61 | #include <netinet/in.h> |
| 62 | #include <netinet/in_var.h> |
| 63 | #include <netinet/if_inarp.h> |
| 64 | #endif /* INET */ |
| 65 | #ifdef INET6 |
| 66 | #include <netinet/in.h> |
| 67 | #include <netinet6/in6_var.h> |
| 68 | #include <netinet6/nd6.h> |
| 69 | #endif /* INET6 */ |
| 70 | |
| 71 | #include <dev/ieee1394/firewire.h> |
| 72 | |
| 73 | #include <dev/ieee1394/firewirereg.h> |
| 74 | #include <dev/ieee1394/iec13213.h> |
| 75 | #include <dev/ieee1394/if_fwipvar.h> |
| 76 | |
| 77 | #define IEEE1394_REASS_TIMEOUT 3 /* 3 sec */ |
| 78 | |
| 79 | #define senderr(e) do { error = (e); goto bad; } while(0/*CONSTCOND*/) |
| 80 | |
| 81 | static int ieee1394_output(struct ifnet *, struct mbuf *, |
| 82 | const struct sockaddr *, const struct rtentry *); |
| 83 | static struct mbuf *ieee1394_reass(struct ifnet *, struct mbuf *, uint16_t); |
| 84 | |
| 85 | static int |
| 86 | ieee1394_output(struct ifnet *ifp, struct mbuf *m0, const struct sockaddr *dst, |
| 87 | const struct rtentry *rt) |
| 88 | { |
| 89 | uint16_t etype = 0; |
| 90 | struct mbuf *m; |
| 91 | int hdrlen, error = 0; |
| 92 | struct mbuf *mcopy = NULL; |
| 93 | struct ieee1394_hwaddr *hwdst, baddr; |
| 94 | const struct ieee1394_hwaddr *myaddr; |
| 95 | #ifdef INET |
| 96 | struct arphdr *ah; |
| 97 | #endif /* INET */ |
| 98 | struct m_tag *mtag; |
| 99 | int unicast; |
| 100 | |
| 101 | if ((ifp->if_flags & (IFF_UP|IFF_RUNNING)) != (IFF_UP|IFF_RUNNING)) |
| 102 | senderr(ENETDOWN); |
| 103 | |
| 104 | /* |
| 105 | * If the queueing discipline needs packet classification, |
| 106 | * do it before prepending link headers. |
| 107 | */ |
| 108 | IFQ_CLASSIFY(&ifp->if_snd, m0, dst->sa_family); |
| 109 | |
| 110 | /* |
| 111 | * For unicast, we make a tag to store the lladdr of the |
| 112 | * destination. This might not be the first time we have seen |
| 113 | * the packet (for instance, the arp code might be trying to |
| 114 | * re-send it after receiving an arp reply) so we only |
| 115 | * allocate a tag if there isn't one there already. For |
| 116 | * multicast, we will eventually use a different tag to store |
| 117 | * the channel number. |
| 118 | */ |
| 119 | unicast = !(m0->m_flags & (M_BCAST | M_MCAST)); |
| 120 | if (unicast) { |
| 121 | mtag = |
| 122 | m_tag_find(m0, MTAG_FIREWIRE_HWADDR, NULL); |
| 123 | if (!mtag) { |
| 124 | mtag = m_tag_get(MTAG_FIREWIRE_HWADDR, |
| 125 | sizeof (struct ieee1394_hwaddr), M_NOWAIT); |
| 126 | if (!mtag) { |
| 127 | error = ENOMEM; |
| 128 | goto bad; |
| 129 | } |
| 130 | m_tag_prepend(m0, mtag); |
| 131 | } |
| 132 | hwdst = (struct ieee1394_hwaddr *)(mtag + 1); |
| 133 | } else { |
| 134 | hwdst = &baddr; |
| 135 | } |
| 136 | |
| 137 | switch (dst->sa_family) { |
| 138 | #ifdef INET |
| 139 | case AF_INET: |
| 140 | if (unicast && |
| 141 | (error = arpresolve(ifp, rt, m0, dst, hwdst, |
| 142 | sizeof(*hwdst))) != 0) |
| 143 | return error == EWOULDBLOCK ? 0 : error; |
| 144 | /* if broadcasting on a simplex interface, loopback a copy */ |
| 145 | if ((m0->m_flags & M_BCAST) && (ifp->if_flags & IFF_SIMPLEX)) |
| 146 | mcopy = m_copy(m0, 0, M_COPYALL); |
| 147 | etype = htons(ETHERTYPE_IP); |
| 148 | break; |
| 149 | case AF_ARP: |
| 150 | ah = mtod(m0, struct arphdr *); |
| 151 | ah->ar_hrd = htons(ARPHRD_IEEE1394); |
| 152 | etype = htons(ETHERTYPE_ARP); |
| 153 | break; |
| 154 | #endif /* INET */ |
| 155 | #ifdef INET6 |
| 156 | case AF_INET6: |
| 157 | if (unicast && (!nd6_storelladdr(ifp, rt, m0, dst, |
| 158 | hwdst->iha_uid, IEEE1394_ADDR_LEN))) { |
| 159 | /* something bad happened */ |
| 160 | return 0; |
| 161 | } |
| 162 | etype = htons(ETHERTYPE_IPV6); |
| 163 | break; |
| 164 | #endif /* INET6 */ |
| 165 | |
| 166 | case pseudo_AF_HDRCMPLT: |
| 167 | case AF_UNSPEC: |
| 168 | /* TODO? */ |
| 169 | default: |
| 170 | printf("%s: can't handle af%d\n" , ifp->if_xname, |
| 171 | dst->sa_family); |
| 172 | senderr(EAFNOSUPPORT); |
| 173 | break; |
| 174 | } |
| 175 | |
| 176 | if (mcopy) |
| 177 | looutput(ifp, mcopy, dst, rt); |
| 178 | myaddr = (const struct ieee1394_hwaddr *)CLLADDR(ifp->if_sadl); |
| 179 | if (ifp->if_bpf) { |
| 180 | struct ieee1394_bpfhdr h; |
| 181 | if (unicast) |
| 182 | memcpy(h.ibh_dhost, hwdst->iha_uid, 8); |
| 183 | else |
| 184 | memcpy(h.ibh_dhost, |
| 185 | ((const struct ieee1394_hwaddr *) |
| 186 | ifp->if_broadcastaddr)->iha_uid, 8); |
| 187 | memcpy(h.ibh_shost, myaddr->iha_uid, 8); |
| 188 | h.ibh_type = etype; |
| 189 | bpf_mtap2(ifp->if_bpf, &h, sizeof(h), m0); |
| 190 | } |
| 191 | if ((ifp->if_flags & IFF_SIMPLEX) && |
| 192 | unicast && |
| 193 | memcmp(hwdst, myaddr, IEEE1394_ADDR_LEN) == 0) |
| 194 | return looutput(ifp, m0, dst, rt); |
| 195 | |
| 196 | /* |
| 197 | * XXX: |
| 198 | * The maximum possible rate depends on the topology. |
| 199 | * So the determination of maxrec and fragmentation should be |
| 200 | * called from the driver after probing the topology map. |
| 201 | */ |
| 202 | if (unicast) { |
| 203 | hdrlen = IEEE1394_GASP_LEN; |
| 204 | hwdst->iha_speed = 0; /* XXX */ |
| 205 | } else |
| 206 | hdrlen = 0; |
| 207 | |
| 208 | if (hwdst->iha_speed > myaddr->iha_speed) |
| 209 | hwdst->iha_speed = myaddr->iha_speed; |
| 210 | if (hwdst->iha_maxrec > myaddr->iha_maxrec) |
| 211 | hwdst->iha_maxrec = myaddr->iha_maxrec; |
| 212 | if (hwdst->iha_maxrec > (8 + hwdst->iha_speed)) |
| 213 | hwdst->iha_maxrec = 8 + hwdst->iha_speed; |
| 214 | if (hwdst->iha_maxrec < 8) |
| 215 | hwdst->iha_maxrec = 8; |
| 216 | |
| 217 | m0 = ieee1394_fragment(ifp, m0, (2<<hwdst->iha_maxrec) - hdrlen, etype); |
| 218 | if (m0 == NULL) |
| 219 | senderr(ENOBUFS); |
| 220 | |
| 221 | while ((m = m0) != NULL) { |
| 222 | m0 = m->m_nextpkt; |
| 223 | |
| 224 | error = if_transmit_lock(ifp, m); |
| 225 | if (error) { |
| 226 | /* mbuf is already freed */ |
| 227 | goto bad; |
| 228 | } |
| 229 | } |
| 230 | return 0; |
| 231 | |
| 232 | bad: |
| 233 | while (m0 != NULL) { |
| 234 | m = m0->m_nextpkt; |
| 235 | m_freem(m0); |
| 236 | m0 = m; |
| 237 | } |
| 238 | |
| 239 | return error; |
| 240 | } |
| 241 | |
| 242 | struct mbuf * |
| 243 | ieee1394_fragment(struct ifnet *ifp, struct mbuf *m0, int maxsize, |
| 244 | uint16_t etype) |
| 245 | { |
| 246 | struct ieee1394com *ic = (struct ieee1394com *)ifp; |
| 247 | int totlen, fraglen, off; |
| 248 | struct mbuf *m, **mp; |
| 249 | struct ieee1394_fraghdr *ifh; |
| 250 | struct ieee1394_unfraghdr *iuh; |
| 251 | |
| 252 | totlen = m0->m_pkthdr.len; |
| 253 | if (totlen + sizeof(struct ieee1394_unfraghdr) <= maxsize) { |
| 254 | M_PREPEND(m0, sizeof(struct ieee1394_unfraghdr), M_DONTWAIT); |
| 255 | if (m0 == NULL) |
| 256 | goto bad; |
| 257 | iuh = mtod(m0, struct ieee1394_unfraghdr *); |
| 258 | iuh->iuh_ft = 0; |
| 259 | iuh->iuh_etype = etype; |
| 260 | return m0; |
| 261 | } |
| 262 | |
| 263 | fraglen = maxsize - sizeof(struct ieee1394_fraghdr); |
| 264 | |
| 265 | M_PREPEND(m0, sizeof(struct ieee1394_fraghdr), M_DONTWAIT); |
| 266 | if (m0 == NULL) |
| 267 | goto bad; |
| 268 | ifh = mtod(m0, struct ieee1394_fraghdr *); |
| 269 | ifh->ifh_ft_size = htons(IEEE1394_FT_MORE | (totlen - 1)); |
| 270 | ifh->ifh_etype_off = etype; |
| 271 | ifh->ifh_dgl = htons(ic->ic_dgl); |
| 272 | ifh->ifh_reserved = 0; |
| 273 | off = fraglen; |
| 274 | mp = &m0->m_nextpkt; |
| 275 | while (off < totlen) { |
| 276 | if (off + fraglen > totlen) |
| 277 | fraglen = totlen - off; |
| 278 | MGETHDR(m, M_DONTWAIT, MT_HEADER); |
| 279 | if (m == NULL) |
| 280 | goto bad; |
| 281 | m->m_flags |= m0->m_flags & (M_BCAST|M_MCAST); /* copy bcast */ |
| 282 | MH_ALIGN(m, sizeof(struct ieee1394_fraghdr)); |
| 283 | m->m_len = sizeof(struct ieee1394_fraghdr); |
| 284 | ifh = mtod(m, struct ieee1394_fraghdr *); |
| 285 | ifh->ifh_ft_size = |
| 286 | htons(IEEE1394_FT_SUBSEQ | IEEE1394_FT_MORE | (totlen - 1)); |
| 287 | ifh->ifh_etype_off = htons(off); |
| 288 | ifh->ifh_dgl = htons(ic->ic_dgl); |
| 289 | ifh->ifh_reserved = 0; |
| 290 | m->m_next = m_copy(m0, sizeof(*ifh) + off, fraglen); |
| 291 | if (m->m_next == NULL) { |
| 292 | m_freem(m); |
| 293 | goto bad; |
| 294 | } |
| 295 | m->m_pkthdr.len = sizeof(*ifh) + fraglen; |
| 296 | off += fraglen; |
| 297 | *mp = m; |
| 298 | mp = &m->m_nextpkt; |
| 299 | } |
| 300 | ifh->ifh_ft_size &= ~htons(IEEE1394_FT_MORE); /* last fragment */ |
| 301 | m_adj(m0, -(m0->m_pkthdr.len - maxsize)); |
| 302 | |
| 303 | ic->ic_dgl++; |
| 304 | return m0; |
| 305 | |
| 306 | bad: |
| 307 | while ((m = m0) != NULL) { |
| 308 | m0 = m->m_nextpkt; |
| 309 | m->m_nextpkt = NULL; |
| 310 | m_freem(m); |
| 311 | } |
| 312 | return NULL; |
| 313 | } |
| 314 | |
| 315 | void |
| 316 | ieee1394_input(struct ifnet *ifp, struct mbuf *m, uint16_t src) |
| 317 | { |
| 318 | pktqueue_t *pktq = NULL; |
| 319 | struct ifqueue *inq; |
| 320 | uint16_t etype; |
| 321 | struct ieee1394_unfraghdr *iuh; |
| 322 | int isr = 0; |
| 323 | |
| 324 | if ((ifp->if_flags & IFF_UP) == 0) { |
| 325 | m_freem(m); |
| 326 | return; |
| 327 | } |
| 328 | if (m->m_len < sizeof(*iuh)) { |
| 329 | if ((m = m_pullup(m, sizeof(*iuh))) == NULL) |
| 330 | return; |
| 331 | } |
| 332 | |
| 333 | iuh = mtod(m, struct ieee1394_unfraghdr *); |
| 334 | |
| 335 | if (ntohs(iuh->iuh_ft) & (IEEE1394_FT_SUBSEQ | IEEE1394_FT_MORE)) { |
| 336 | if ((m = ieee1394_reass(ifp, m, src)) == NULL) |
| 337 | return; |
| 338 | iuh = mtod(m, struct ieee1394_unfraghdr *); |
| 339 | } |
| 340 | etype = ntohs(iuh->iuh_etype); |
| 341 | |
| 342 | /* strip off the ieee1394 header */ |
| 343 | m_adj(m, sizeof(*iuh)); |
| 344 | if (ifp->if_bpf) { |
| 345 | struct ieee1394_bpfhdr h; |
| 346 | struct m_tag *mtag; |
| 347 | const struct ieee1394_hwaddr *myaddr; |
| 348 | |
| 349 | mtag = m_tag_find(m, MTAG_FIREWIRE_SENDER_EUID, 0); |
| 350 | if (mtag) |
| 351 | memcpy(h.ibh_shost, mtag + 1, 8); |
| 352 | else |
| 353 | memset(h.ibh_shost, 0, 8); |
| 354 | if (m->m_flags & M_BCAST) |
| 355 | memcpy(h.ibh_dhost, |
| 356 | ((const struct ieee1394_hwaddr *) |
| 357 | ifp->if_broadcastaddr)->iha_uid, 8); |
| 358 | else { |
| 359 | myaddr = |
| 360 | (const struct ieee1394_hwaddr *)CLLADDR(ifp->if_sadl); |
| 361 | memcpy(h.ibh_dhost, myaddr->iha_uid, 8); |
| 362 | } |
| 363 | h.ibh_type = htons(etype); |
| 364 | bpf_mtap2(ifp->if_bpf, &h, sizeof(h), m); |
| 365 | } |
| 366 | |
| 367 | switch (etype) { |
| 368 | #ifdef INET |
| 369 | case ETHERTYPE_IP: |
| 370 | pktq = ip_pktq; |
| 371 | break; |
| 372 | |
| 373 | case ETHERTYPE_ARP: |
| 374 | isr = NETISR_ARP; |
| 375 | inq = &arpintrq; |
| 376 | break; |
| 377 | #endif /* INET */ |
| 378 | |
| 379 | #ifdef INET6 |
| 380 | case ETHERTYPE_IPV6: |
| 381 | pktq = ip6_pktq; |
| 382 | break; |
| 383 | #endif /* INET6 */ |
| 384 | |
| 385 | default: |
| 386 | m_freem(m); |
| 387 | return; |
| 388 | } |
| 389 | |
| 390 | if (__predict_true(pktq)) { |
| 391 | if (__predict_false(!pktq_enqueue(pktq, m, 0))) { |
| 392 | m_freem(m); |
| 393 | } |
| 394 | return; |
| 395 | } |
| 396 | |
| 397 | IFQ_LOCK(inq); |
| 398 | if (IF_QFULL(inq)) { |
| 399 | IF_DROP(inq); |
| 400 | IFQ_UNLOCK(inq); |
| 401 | m_freem(m); |
| 402 | } else { |
| 403 | IF_ENQUEUE(inq, m); |
| 404 | IFQ_UNLOCK(inq); |
| 405 | schednetisr(isr); |
| 406 | } |
| 407 | } |
| 408 | |
| 409 | static struct mbuf * |
| 410 | ieee1394_reass(struct ifnet *ifp, struct mbuf *m0, uint16_t src) |
| 411 | { |
| 412 | struct ieee1394com *ic = (struct ieee1394com *)ifp; |
| 413 | struct ieee1394_fraghdr *ifh; |
| 414 | struct ieee1394_unfraghdr *iuh; |
| 415 | struct ieee1394_reassq *rq; |
| 416 | struct ieee1394_reass_pkt *rp, *trp, *nrp = NULL; |
| 417 | int len; |
| 418 | uint16_t etype, off, ftype, size, dgl; |
| 419 | uint32_t id; |
| 420 | |
| 421 | if (m0->m_len < sizeof(*ifh)) { |
| 422 | if ((m0 = m_pullup(m0, sizeof(*ifh))) == NULL) |
| 423 | return NULL; |
| 424 | } |
| 425 | ifh = mtod(m0, struct ieee1394_fraghdr *); |
| 426 | m_adj(m0, sizeof(*ifh)); |
| 427 | size = ntohs(ifh->ifh_ft_size); |
| 428 | ftype = size & (IEEE1394_FT_SUBSEQ | IEEE1394_FT_MORE); |
| 429 | size = (size & ~ftype) + 1; |
| 430 | dgl = ntohs(ifh->ifh_dgl); |
| 431 | len = m0->m_pkthdr.len; |
| 432 | id = dgl | (src << 16); |
| 433 | if (ftype & IEEE1394_FT_SUBSEQ) { |
| 434 | m_tag_delete_chain(m0, NULL); |
| 435 | m0->m_flags &= ~M_PKTHDR; |
| 436 | etype = 0; |
| 437 | off = ntohs(ifh->ifh_etype_off); |
| 438 | } else { |
| 439 | etype = ifh->ifh_etype_off; |
| 440 | off = 0; |
| 441 | } |
| 442 | |
| 443 | for (rq = LIST_FIRST(&ic->ic_reassq); ; rq = LIST_NEXT(rq, rq_node)) { |
| 444 | if (rq == NULL) { |
| 445 | /* |
| 446 | * Create a new reassemble queue head for the node. |
| 447 | */ |
| 448 | rq = malloc(sizeof(*rq), M_FTABLE, M_NOWAIT); |
| 449 | if (rq == NULL) { |
| 450 | m_freem(m0); |
| 451 | return NULL; |
| 452 | } |
| 453 | rq->fr_id = id; |
| 454 | LIST_INIT(&rq->rq_pkt); |
| 455 | LIST_INSERT_HEAD(&ic->ic_reassq, rq, rq_node); |
| 456 | break; |
| 457 | } |
| 458 | if (rq->fr_id == id) |
| 459 | break; |
| 460 | } |
| 461 | for (rp = LIST_FIRST(&rq->rq_pkt); rp != NULL; rp = nrp) { |
| 462 | nrp = LIST_NEXT(rp, rp_next); |
| 463 | if (rp->rp_dgl != dgl) |
| 464 | continue; |
| 465 | /* |
| 466 | * sanity check: |
| 467 | * datagram size must be same for all fragments, and |
| 468 | * no overlap is allowed. |
| 469 | */ |
| 470 | if (rp->rp_size != size || |
| 471 | (off < rp->rp_off + rp->rp_len && off + len > rp->rp_off)) { |
| 472 | /* |
| 473 | * This happens probably due to wrapping dgl value. |
| 474 | * Destroy all previously received fragment and |
| 475 | * enqueue current fragment. |
| 476 | */ |
| 477 | for (rp = LIST_FIRST(&rq->rq_pkt); rp != NULL; |
| 478 | rp = nrp) { |
| 479 | nrp = LIST_NEXT(rp, rp_next); |
| 480 | if (rp->rp_dgl == dgl) { |
| 481 | LIST_REMOVE(rp, rp_next); |
| 482 | m_freem(rp->rp_m); |
| 483 | free(rp, M_FTABLE); |
| 484 | } |
| 485 | } |
| 486 | break; |
| 487 | } |
| 488 | if (rp->rp_off + rp->rp_len == off) { |
| 489 | /* |
| 490 | * All the subsequent fragments received in sequence |
| 491 | * come here. |
| 492 | * Concatinate mbuf to previous one instead of |
| 493 | * allocating new reassemble queue structure, |
| 494 | * and try to merge more with the subsequent fragment |
| 495 | * in the queue. |
| 496 | */ |
| 497 | m_cat(rp->rp_m, m0); |
| 498 | rp->rp_len += len; |
| 499 | while (rp->rp_off + rp->rp_len < size && |
| 500 | nrp != NULL && nrp->rp_dgl == dgl && |
| 501 | nrp->rp_off == rp->rp_off + rp->rp_len) { |
| 502 | LIST_REMOVE(nrp, rp_next); |
| 503 | m_cat(rp->rp_m, nrp->rp_m); |
| 504 | rp->rp_len += nrp->rp_len; |
| 505 | free(nrp, M_FTABLE); |
| 506 | nrp = LIST_NEXT(rp, rp_next); |
| 507 | } |
| 508 | m0 = NULL; /* mark merged */ |
| 509 | break; |
| 510 | } |
| 511 | if (off + m0->m_pkthdr.len == rp->rp_off) { |
| 512 | m_cat(m0, rp->rp_m); |
| 513 | rp->rp_m = m0; |
| 514 | rp->rp_off = off; |
| 515 | rp->rp_etype = etype; /* over writing trust etype */ |
| 516 | rp->rp_len += len; |
| 517 | m0 = NULL; /* mark merged */ |
| 518 | break; |
| 519 | } |
| 520 | if (rp->rp_off > off) { |
| 521 | /* insert before rp */ |
| 522 | nrp = rp; |
| 523 | break; |
| 524 | } |
| 525 | if (nrp == NULL || nrp->rp_dgl != dgl) { |
| 526 | /* insert after rp */ |
| 527 | nrp = NULL; |
| 528 | break; |
| 529 | } |
| 530 | } |
| 531 | if (m0 == NULL) { |
| 532 | if (rp->rp_off != 0 || rp->rp_len != size) |
| 533 | return NULL; |
| 534 | /* fragment done */ |
| 535 | LIST_REMOVE(rp, rp_next); |
| 536 | m0 = rp->rp_m; |
| 537 | m0->m_pkthdr.len = rp->rp_len; |
| 538 | M_PREPEND(m0, sizeof(*iuh), M_DONTWAIT); |
| 539 | if (m0 != NULL) { |
| 540 | iuh = mtod(m0, struct ieee1394_unfraghdr *); |
| 541 | iuh->iuh_ft = 0; |
| 542 | iuh->iuh_etype = rp->rp_etype; |
| 543 | } |
| 544 | free(rp, M_FTABLE); |
| 545 | return m0; |
| 546 | } |
| 547 | |
| 548 | /* |
| 549 | * New fragment received. Allocate reassemble queue structure. |
| 550 | */ |
| 551 | trp = malloc(sizeof(*trp), M_FTABLE, M_NOWAIT); |
| 552 | if (trp == NULL) { |
| 553 | m_freem(m0); |
| 554 | return NULL; |
| 555 | } |
| 556 | trp->rp_m = m0; |
| 557 | trp->rp_size = size; |
| 558 | trp->rp_etype = etype; /* valid only if off==0 */ |
| 559 | trp->rp_off = off; |
| 560 | trp->rp_dgl = dgl; |
| 561 | trp->rp_len = len; |
| 562 | trp->rp_ttl = IEEE1394_REASS_TIMEOUT; |
| 563 | if (trp->rp_ttl <= ifp->if_timer) |
| 564 | trp->rp_ttl = ifp->if_timer + 1; |
| 565 | |
| 566 | if (rp == NULL) { |
| 567 | /* first fragment for the dgl */ |
| 568 | LIST_INSERT_HEAD(&rq->rq_pkt, trp, rp_next); |
| 569 | } else if (nrp == NULL) { |
| 570 | /* no next fragment for the dgl */ |
| 571 | LIST_INSERT_AFTER(rp, trp, rp_next); |
| 572 | } else { |
| 573 | /* there is a hole */ |
| 574 | LIST_INSERT_BEFORE(nrp, trp, rp_next); |
| 575 | } |
| 576 | return NULL; |
| 577 | } |
| 578 | |
| 579 | void |
| 580 | ieee1394_drain(struct ifnet *ifp) |
| 581 | { |
| 582 | struct ieee1394com *ic = (struct ieee1394com *)ifp; |
| 583 | struct ieee1394_reassq *rq; |
| 584 | struct ieee1394_reass_pkt *rp; |
| 585 | |
| 586 | while ((rq = LIST_FIRST(&ic->ic_reassq)) != NULL) { |
| 587 | LIST_REMOVE(rq, rq_node); |
| 588 | while ((rp = LIST_FIRST(&rq->rq_pkt)) != NULL) { |
| 589 | LIST_REMOVE(rp, rp_next); |
| 590 | m_freem(rp->rp_m); |
| 591 | free(rp, M_FTABLE); |
| 592 | } |
| 593 | free(rq, M_FTABLE); |
| 594 | } |
| 595 | } |
| 596 | |
| 597 | void |
| 598 | ieee1394_watchdog(struct ifnet *ifp) |
| 599 | { |
| 600 | struct ieee1394com *ic = (struct ieee1394com *)ifp; |
| 601 | struct ieee1394_reassq *rq; |
| 602 | struct ieee1394_reass_pkt *rp, *nrp; |
| 603 | int dec; |
| 604 | |
| 605 | dec = (ifp->if_timer > 0) ? ifp->if_timer : 1; |
| 606 | for (rq = LIST_FIRST(&ic->ic_reassq); rq != NULL; |
| 607 | rq = LIST_NEXT(rq, rq_node)) { |
| 608 | for (rp = LIST_FIRST(&rq->rq_pkt); rp != NULL; rp = nrp) { |
| 609 | nrp = LIST_NEXT(rp, rp_next); |
| 610 | if (rp->rp_ttl >= dec) |
| 611 | rp->rp_ttl -= dec; |
| 612 | else { |
| 613 | LIST_REMOVE(rp, rp_next); |
| 614 | m_freem(rp->rp_m); |
| 615 | free(rp, M_FTABLE); |
| 616 | } |
| 617 | } |
| 618 | } |
| 619 | } |
| 620 | |
| 621 | const char * |
| 622 | ieee1394_sprintf(const uint8_t *laddr) |
| 623 | { |
| 624 | static char buf[3*8]; |
| 625 | |
| 626 | snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x" , |
| 627 | laddr[0], laddr[1], laddr[2], laddr[3], |
| 628 | laddr[4], laddr[5], laddr[6], laddr[7]); |
| 629 | return buf; |
| 630 | } |
| 631 | |
| 632 | void |
| 633 | ieee1394_ifattach(struct ifnet *ifp, const struct ieee1394_hwaddr *hwaddr) |
| 634 | { |
| 635 | struct ieee1394_hwaddr *baddr; |
| 636 | struct ieee1394com *ic = (struct ieee1394com *)ifp; |
| 637 | |
| 638 | ifp->if_type = IFT_IEEE1394; |
| 639 | ifp->if_hdrlen = sizeof(struct ieee1394_header); |
| 640 | ifp->if_dlt = DLT_EN10MB; /* XXX */ |
| 641 | ifp->if_mtu = IEEE1394MTU; |
| 642 | ifp->if_output = ieee1394_output; |
| 643 | ifp->if_drain = ieee1394_drain; |
| 644 | ifp->if_watchdog = ieee1394_watchdog; |
| 645 | ifp->if_timer = 1; |
| 646 | if (ifp->if_baudrate == 0) |
| 647 | ifp->if_baudrate = IF_Mbps(100); |
| 648 | |
| 649 | if_set_sadl(ifp, hwaddr, sizeof(struct ieee1394_hwaddr), true); |
| 650 | |
| 651 | baddr = malloc(ifp->if_addrlen, M_DEVBUF, M_WAITOK); |
| 652 | memset(baddr->iha_uid, 0xff, IEEE1394_ADDR_LEN); |
| 653 | baddr->iha_speed = 0; /*XXX: how to determine the speed for bcast? */ |
| 654 | baddr->iha_maxrec = 512 << baddr->iha_speed; |
| 655 | memset(baddr->iha_offset, 0, sizeof(baddr->iha_offset)); |
| 656 | ifp->if_broadcastaddr = (uint8_t *)baddr; |
| 657 | LIST_INIT(&ic->ic_reassq); |
| 658 | bpf_attach(ifp, DLT_APPLE_IP_OVER_IEEE1394, |
| 659 | sizeof(struct ieee1394_hwaddr)); |
| 660 | } |
| 661 | |
| 662 | void |
| 663 | ieee1394_ifdetach(struct ifnet *ifp) |
| 664 | { |
| 665 | ieee1394_drain(ifp); |
| 666 | bpf_detach(ifp); |
| 667 | free(__UNCONST(ifp->if_broadcastaddr), M_DEVBUF); |
| 668 | ifp->if_broadcastaddr = NULL; |
| 669 | } |
| 670 | |
| 671 | int |
| 672 | ieee1394_ioctl(struct ifnet *ifp, u_long cmd, void *data) |
| 673 | { |
| 674 | struct ifreq *ifr = (struct ifreq *)data; |
| 675 | struct ifaddr *ifa = (struct ifaddr *)data; |
| 676 | int error = 0; |
| 677 | |
| 678 | switch (cmd) { |
| 679 | case SIOCINITIFADDR: |
| 680 | ifp->if_flags |= IFF_UP; |
| 681 | switch (ifa->ifa_addr->sa_family) { |
| 682 | #ifdef INET |
| 683 | case AF_INET: |
| 684 | if ((error = (*ifp->if_init)(ifp)) != 0) |
| 685 | break; |
| 686 | arp_ifinit(ifp, ifa); |
| 687 | break; |
| 688 | #endif /* INET */ |
| 689 | default: |
| 690 | error = (*ifp->if_init)(ifp); |
| 691 | break; |
| 692 | } |
| 693 | break; |
| 694 | |
| 695 | case SIOCSIFMTU: |
| 696 | if (ifr->ifr_mtu > IEEE1394MTU) |
| 697 | error = EINVAL; |
| 698 | else if ((error = ifioctl_common(ifp, cmd, data)) == ENETRESET) |
| 699 | error = 0; |
| 700 | break; |
| 701 | |
| 702 | default: |
| 703 | error = ifioctl_common(ifp, cmd, data); |
| 704 | break; |
| 705 | } |
| 706 | |
| 707 | return error; |
| 708 | } |
| 709 | |