/// <summary>
        /// Adds broadcast/mesh headers and sends SDU to MAC
        /// </summary>
        /// <returns>True on success</returns>
        private void SendData(
            UInt16 orgAddr,
            UInt16 tgtAddr,
            UInt16 nextHopAddr,
            ref Frame sdu,
            Byte sduHandle,
            DataConfirmHandler handler)
        {
            bool success = false;
            if (orgAddr != cInvalidShortAddr)
            {
                bool ok = true;
                bool multicast = false;
                if (IsMulticast(tgtAddr))
                {
                    multicast = true;
                    Messages6LoWPAN.BroadcastHeader bh = new Messages6LoWPAN.BroadcastHeader();
                    bh.seqNo = IncSeqNoBroadcast();
                    ok = bh.WriteToFrameHeader(sdu);
                }

                if (ok && (multicast || (tgtAddr != nextHopAddr)))
                {
                    // add mesh header
                    Messages6LoWPAN.MeshHeader mh = new Messages6LoWPAN.MeshHeader();
                    mh.originatorAddress = orgAddr;
                    mh.finalAddress = tgtAddr;
                    mh.HopsLeft = cDefaultHopLimit;
                    ok = mh.WriteToFrameHeader(sdu);
                }

                if (ok)
                {
                    MacDataRequest(nextHopAddr, ref sdu, sduHandle, handler);
                    success = true;
                }
            }

            Frame.Release(ref sdu);
            if (!success && handler != null)
                handler.Invoke(_net, sduHandle, Status.Busy);
        }
        private void HandleData(
            UInt16 srcAddr,
            UInt16 dstAddr,
            ref Frame sdu)
        {
            Messages6LoWPAN.Dispatch dispatch;
            if (!Messages6LoWPAN.GetType(sdu, out dispatch))
                return;

            bool consume = false; // receive locally
            bool forward = false; // forward to another node

            UInt16 origAddr = srcAddr; // originator
            UInt16 targetAddr = dstAddr; // target
            bool haveMeshHeader = false;
            Messages6LoWPAN.MeshHeader mh = new Messages6LoWPAN.MeshHeader();
            if (dispatch == Messages6LoWPAN.Dispatch.Mesh)
            {
                if (!mh.ReadFromFrameHeader(sdu) || !Messages6LoWPAN.GetType(sdu, out dispatch))
                    return;
                haveMeshHeader = true;
                origAddr = mh.originatorAddress;
                targetAddr = mh.finalAddress;
            }

            bool haveBroadcastHeader = false;
            Messages6LoWPAN.BroadcastHeader bh = new Messages6LoWPAN.BroadcastHeader();
            if (dispatch == Messages6LoWPAN.Dispatch.IpPacket && bh.ReadFromFrameHeader(sdu))
            {
                haveBroadcastHeader = true;
                bool usefull = _routingTable.CheckBroadcastSeqNo(origAddr, bh.seqNo) && (origAddr != _addrShort);
                if (!usefull || !haveMeshHeader || !Messages6LoWPAN.GetType(sdu, out dispatch))
                    return;
            }

            bool multicast = IsMulticast(targetAddr);
            consume = (targetAddr == _addrShort || multicast);
            forward = (targetAddr != _addrShort || multicast) && haveMeshHeader && (mh.HopsLeft > 0);

            if (forward)
            {
                mh.HopsLeft--;
                // currently we always need to reallocate a frame to be forwarded, since mac header space is worst case assumption
                Frame frame = Frame.GetFrame(_macHeader + Messages6LoWPAN.MeshHeader.cLengthMax + Messages6LoWPAN.BroadcastHeader.cLength,
                    sdu.LengthDataUsed + _macTailer);
                frame.WriteToBack(sdu);

                bool ok = true;
                UInt16 nextHop;
                if (multicast)
                {
                    nextHop = cBroadcastShortAddr;
                    ok = haveBroadcastHeader && bh.WriteToFrameHeader(frame);
                }
                else
                {
                    bool routeHasError;
                    ok = _routingTable.GetRoute(targetAddr, out nextHop, out routeHasError);
                    if (!ok || routeHasError)
                    {
                        SendRouteError(origAddr, targetAddr, !routeHasError);
                    }
                }

                ok &= mh.WriteToFrameHeader(frame);
                if (ok)
                {
                    MacDataRequest(nextHop, ref frame);
                }

                Frame.Release(ref frame);
            }

            if (consume)
            {
                if (dispatch == Messages6LoWPAN.Dispatch.NonLowPan)
                {
                    Message.Type type;
                    if (Message.GetType(sdu, out type))
                    {
                        switch (type)
                        {
                            case Message.Type.Data:
                                {
                                    Message.Data data = new Message.Data();
                                    if (data.ReadFromFrameHeader(sdu))
                                    {
                                        _data.HandleDataIndication(origAddr, targetAddr, ref sdu);
                                    }
                                    break;
                                }
                            case Message.Type.DiscoveryReply:
                                {
                                    Message.DiscoveryReply drep;
                                    if (_isAddrServer && drep.ReadFromFrame(sdu))
                                    {
                                        _addrServer.HandleDiscoveryReply(origAddr);
                                    }
                                    break;
                                }
                        }
                    }
                }
                else
                { // must be 6LoWPAN frame
                    _data.HandleDataIndication6Low(origAddr, targetAddr, ref sdu);
                }
            }

            Frame.Release(ref sdu);
        }