<commit_before>// Copyright Daniel Wallin 2007. Use, modification and distribution is
// subject to the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <libtorrent/extensions.hpp>
#include <libtorrent/entry.hpp>
#include <libtorrent/lazy_entry.hpp>
#include <libtorrent/peer_request.hpp>
#include <libtorrent/disk_buffer_holder.hpp>
#include <libtorrent/bitfield.hpp>
#include <boost/python.hpp>

using namespace boost::python;
using namespace libtorrent;

namespace
{
  struct peer_plugin_wrap : peer_plugin, wrapper<peer_plugin>
  {
      void add_handshake(entry& e)
      {
          if (override f = this->get_override("add_handshake"))
              e = call<entry>(f.ptr(), e);
          else
              peer_plugin::add_handshake(e);
      }

      void default_add_handshake(entry& e)
      {
          this->peer_plugin::add_handshake(e);
      }

      bool on_handshake(char const* reserved_bits)
      {
          if (override f = this->get_override("on_handshake"))
              return f();
          else
              return peer_plugin::on_handshake(reserved_bits);
      }

      bool default_on_handshake(char const* reserved_bits)
      {
          return this->peer_plugin::on_handshake(reserved_bits);
      }

      bool on_extension_handshake(lazy_entry const& e)
      {
          if (override f = this->get_override("on_extension_handshake"))
              return f(e);
          else
              return peer_plugin::on_extension_handshake(e);
      }

      bool default_on_extension_handshake(lazy_entry const& e)
      {
          return this->peer_plugin::on_extension_handshake(e);
      }

      bool on_choke()
      {
          if (override f = this->get_override("on_choke"))
              return f();
          else
              return peer_plugin::on_choke();
      }

      bool default_on_choke()
      {
          return this->peer_plugin::on_choke();
      }

      bool on_unchoke()
      {
          if (override f = this->get_override("on_unchoke"))
              return f();
          else
              return peer_plugin::on_unchoke();
      }

      bool default_on_unchoke()
      {
          return this->peer_plugin::on_unchoke();
      }

      bool on_interested()
      {
          if (override f = this->get_override("on_interested"))
              return f();
          else
              return peer_plugin::on_interested();
      }

      bool default_on_interested()
      {
          return this->peer_plugin::on_interested();
      }

      bool on_not_interested()
      {
          if (override f = this->get_override("on_not_interested"))
              return f();
          else
              return peer_plugin::on_not_interested();
      }

      bool default_on_not_interested()
      {
          return this->peer_plugin::on_not_interested();
      }

      bool on_have(int index)
      {
          if (override f = this->get_override("on_have"))
              return f(index);
          else
              return peer_plugin::on_have(index);
      }

      bool default_on_have(int index)
      {
          return this->peer_plugin::on_have(index);
      }

      bool on_bitfield(list _bf)
      {
          //Convert list to a bitfield
          bitfield bf(len(_bf));
          for (int i = 0; i < len(_bf); ++i)
          {
              if (_bf[i])
                  bf.set_bit(i);
              else
                  bf.clear_bit(i);
          }
          if (override f = this->get_override("on_bitfield"))
              return f(bf);
          else
              return peer_plugin::on_bitfield(bf);
      }

      bool default_on_bitfield(const bitfield &bf)
      {
          return this->peer_plugin::on_bitfield(bf);
      }

      bool on_request(peer_request const& req)
      {
          if (override f = this->get_override("on_request"))
              return f(req);
          else
              return peer_plugin::on_request(req);
      }

      bool default_on_request(peer_request const& req)
      {
          return this->peer_plugin::on_request(req);
      }

      bool on_piece(peer_request const& piece, disk_buffer_holder& data)
      {
          if (override f = this->get_override("on_piece"))
              return f(piece, data);
          else
              return peer_plugin::on_piece(piece, data);
      }

      bool default_on_piece(peer_request const& piece, disk_buffer_holder& data)
      {
          return this->peer_plugin::on_piece(piece, data);
      }

      bool on_cancel(peer_request const& req)
      {
          if (override f = this->get_override("on_cancel"))
              return f(req);
          else
              return peer_plugin::on_cancel(req);
      }

      bool default_on_cancel(peer_request const& req)
      {
          return this->peer_plugin::on_cancel(req);
      }

      bool on_extended(int length, int msg, buffer::const_interval body)
      {
          if (override f = this->get_override("on_extended"))
              return f(length, msg, body);
          else
              return peer_plugin::on_extended(length, msg, body);
      }

      bool default_on_extended(int length, int msg, buffer::const_interval body)
      {
          return this->peer_plugin::on_extended(length, msg, body);
      }

      bool on_unknown_message(int length, int msg, buffer::const_interval body)
      {
          if (override f = this->get_override("on_unknown_message"))
              return f(length, msg, body);
          else
              return peer_plugin::on_unknown_message(length, msg, body);
      }

      bool default_on_unknown_message(int length, int msg, buffer::const_interval body)
      {
          return this->peer_plugin::on_unknown_message(length, msg, body);
      }

      void on_piece_pass(int index)
      {
          if (override f = this->get_override("on_piece_pass"))
              f(index);
          else
              peer_plugin::on_piece_pass(index);
      }

      void default_on_piece_pass(int index)
      {
          this->peer_plugin::on_piece_pass(index);
      }

      void on_piece_failed(int index)
      {
          if (override f = this->get_override("on_piece_failed"))
              f(index);
          else
              peer_plugin::on_piece_failed(index);
      }

      void default_on_piece_failed(int index)
      {
          this->peer_plugin::on_piece_failed(index);
      }

      void tick()
      {
          if (override f = this->get_override("tick"))
              f();
          else
              peer_plugin::tick();
      }

      void default_tick()
      {
          this->peer_plugin::tick();
      }

      bool write_request(peer_request const& req)
      {
          if (override f = this->get_override("write_request"))
              return f(req);
          else
              return peer_plugin::write_request(req);
      }

      bool default_write_request(peer_request const& req)
      {
          return this->peer_plugin::write_request(req);
      }
  };

  object get_buffer()
  {
      static char const data[] = "foobar";
      return object(handle<>(PyBuffer_FromMemory((void*)data, 6)));
  }

} // namespace unnamed

void bind_peer_plugin()
{
    class_<
        peer_plugin_wrap, boost::shared_ptr<peer_plugin_wrap>, boost::noncopyable
    >("peer_plugin")
        .def(
            "add_handshake"
          , &peer_plugin::add_handshake, &peer_plugin_wrap::default_add_handshake
        )
        .def(
            "on_handshake"
          , &peer_plugin::on_handshake, &peer_plugin_wrap::default_on_handshake
        )
        .def(
            "on_extension_handshake"
          , &peer_plugin::on_extension_handshake
          , &peer_plugin_wrap::default_on_extension_handshake
        )
        .def(
            "on_choke"
          , &peer_plugin::on_choke, &peer_plugin_wrap::default_on_choke
        )
        .def(
            "on_unchoke"
          , &peer_plugin::on_unchoke, &peer_plugin_wrap::default_on_unchoke
        )
        .def(
            "on_interested"
          , &peer_plugin::on_interested, &peer_plugin_wrap::default_on_interested
        )
        .def(
            "on_not_interested"
          , &peer_plugin::on_not_interested, &peer_plugin_wrap::default_on_not_interested
        )
        .def(
            "on_have"
          , &peer_plugin::on_have, &peer_plugin_wrap::default_on_have
        )
        .def(
            "on_bitfield"
          , &peer_plugin::on_bitfield, &peer_plugin_wrap::default_on_bitfield
        )
        .def(
            "on_request"
          , &peer_plugin::on_request, &peer_plugin_wrap::default_on_request
        )
        .def(
            "on_piece"
          , &peer_plugin::on_piece, &peer_plugin_wrap::default_on_piece
        )
        .def(
            "on_cancel"
          , &peer_plugin::on_cancel, &peer_plugin_wrap::default_on_cancel
        )
        .def(
            "on_piece_pass"
          , &peer_plugin::on_piece_pass, &peer_plugin_wrap::default_on_piece_pass
        )
        .def(
            "on_piece_failed"
          , &peer_plugin::on_piece_failed, &peer_plugin_wrap::default_on_piece_failed
        )
        .def(
            "tick"
          , &peer_plugin::tick, &peer_plugin_wrap::default_tick
        )
        .def(
            "write_request"
          , &peer_plugin::write_request, &peer_plugin_wrap::default_write_request
        )
        // These seem to make VC7.1 freeze. Needs special handling.

        /*.def(
            "on_extended"
          , &peer_plugin::on_extended, &peer_plugin_wrap::default_on_extended
        )
        .def(
            "on_unknown_message"
          , &peer_plugin::on_unknown_message, &peer_plugin_wrap::default_on_unknown_message
        )*/
        ;

    def("get_buffer", &get_buffer);
}

<commit_msg>fix build for FreeBSD<commit_after>// Copyright Daniel Wallin 2007. Use, modification and distribution is
// subject to the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <cctype>
#include <iostream>

#include <libtorrent/extensions.hpp>
#include <libtorrent/entry.hpp>
#include <libtorrent/lazy_entry.hpp>
#include <libtorrent/peer_request.hpp>
#include <libtorrent/disk_buffer_holder.hpp>
#include <libtorrent/bitfield.hpp>
#include <boost/python.hpp>

using namespace boost::python;
using namespace libtorrent;

namespace
{
  struct peer_plugin_wrap : peer_plugin, wrapper<peer_plugin>
  {
      void add_handshake(entry& e)
      {
          if (override f = this->get_override("add_handshake"))
              e = call<entry>(f.ptr(), e);
          else
              peer_plugin::add_handshake(e);
      }

      void default_add_handshake(entry& e)
      {
          this->peer_plugin::add_handshake(e);
      }

      bool on_handshake(char const* reserved_bits)
      {
          if (override f = this->get_override("on_handshake"))
              return f();
          else
              return peer_plugin::on_handshake(reserved_bits);
      }

      bool default_on_handshake(char const* reserved_bits)
      {
          return this->peer_plugin::on_handshake(reserved_bits);
      }

      bool on_extension_handshake(lazy_entry const& e)
      {
          if (override f = this->get_override("on_extension_handshake"))
              return f(e);
          else
              return peer_plugin::on_extension_handshake(e);
      }

      bool default_on_extension_handshake(lazy_entry const& e)
      {
          return this->peer_plugin::on_extension_handshake(e);
      }

      bool on_choke()
      {
          if (override f = this->get_override("on_choke"))
              return f();
          else
              return peer_plugin::on_choke();
      }

      bool default_on_choke()
      {
          return this->peer_plugin::on_choke();
      }

      bool on_unchoke()
      {
          if (override f = this->get_override("on_unchoke"))
              return f();
          else
              return peer_plugin::on_unchoke();
      }

      bool default_on_unchoke()
      {
          return this->peer_plugin::on_unchoke();
      }

      bool on_interested()
      {
          if (override f = this->get_override("on_interested"))
              return f();
          else
              return peer_plugin::on_interested();
      }

      bool default_on_interested()
      {
          return this->peer_plugin::on_interested();
      }

      bool on_not_interested()
      {
          if (override f = this->get_override("on_not_interested"))
              return f();
          else
              return peer_plugin::on_not_interested();
      }

      bool default_on_not_interested()
      {
          return this->peer_plugin::on_not_interested();
      }

      bool on_have(int index)
      {
          if (override f = this->get_override("on_have"))
              return f(index);
          else
              return peer_plugin::on_have(index);
      }

      bool default_on_have(int index)
      {
          return this->peer_plugin::on_have(index);
      }

      bool on_bitfield(list _bf)
      {
          //Convert list to a bitfield
          bitfield bf(len(_bf));
          for (int i = 0; i < len(_bf); ++i)
          {
              if (_bf[i])
                  bf.set_bit(i);
              else
                  bf.clear_bit(i);
          }
          if (override f = this->get_override("on_bitfield"))
              return f(bf);
          else
              return peer_plugin::on_bitfield(bf);
      }

      bool default_on_bitfield(const bitfield &bf)
      {
          return this->peer_plugin::on_bitfield(bf);
      }

      bool on_request(peer_request const& req)
      {
          if (override f = this->get_override("on_request"))
              return f(req);
          else
              return peer_plugin::on_request(req);
      }

      bool default_on_request(peer_request const& req)
      {
          return this->peer_plugin::on_request(req);
      }

      bool on_piece(peer_request const& piece, disk_buffer_holder& data)
      {
          if (override f = this->get_override("on_piece"))
              return f(piece, data);
          else
              return peer_plugin::on_piece(piece, data);
      }

      bool default_on_piece(peer_request const& piece, disk_buffer_holder& data)
      {
          return this->peer_plugin::on_piece(piece, data);
      }

      bool on_cancel(peer_request const& req)
      {
          if (override f = this->get_override("on_cancel"))
              return f(req);
          else
              return peer_plugin::on_cancel(req);
      }

      bool default_on_cancel(peer_request const& req)
      {
          return this->peer_plugin::on_cancel(req);
      }

      bool on_extended(int length, int msg, buffer::const_interval body)
      {
          if (override f = this->get_override("on_extended"))
              return f(length, msg, body);
          else
              return peer_plugin::on_extended(length, msg, body);
      }

      bool default_on_extended(int length, int msg, buffer::const_interval body)
      {
          return this->peer_plugin::on_extended(length, msg, body);
      }

      bool on_unknown_message(int length, int msg, buffer::const_interval body)
      {
          if (override f = this->get_override("on_unknown_message"))
              return f(length, msg, body);
          else
              return peer_plugin::on_unknown_message(length, msg, body);
      }

      bool default_on_unknown_message(int length, int msg, buffer::const_interval body)
      {
          return this->peer_plugin::on_unknown_message(length, msg, body);
      }

      void on_piece_pass(int index)
      {
          if (override f = this->get_override("on_piece_pass"))
              f(index);
          else
              peer_plugin::on_piece_pass(index);
      }

      void default_on_piece_pass(int index)
      {
          this->peer_plugin::on_piece_pass(index);
      }

      void on_piece_failed(int index)
      {
          if (override f = this->get_override("on_piece_failed"))
              f(index);
          else
              peer_plugin::on_piece_failed(index);
      }

      void default_on_piece_failed(int index)
      {
          this->peer_plugin::on_piece_failed(index);
      }

      void tick()
      {
          if (override f = this->get_override("tick"))
              f();
          else
              peer_plugin::tick();
      }

      void default_tick()
      {
          this->peer_plugin::tick();
      }

      bool write_request(peer_request const& req)
      {
          if (override f = this->get_override("write_request"))
              return f(req);
          else
              return peer_plugin::write_request(req);
      }

      bool default_write_request(peer_request const& req)
      {
          return this->peer_plugin::write_request(req);
      }
  };

  object get_buffer()
  {
      static char const data[] = "foobar";
      return object(handle<>(PyBuffer_FromMemory((void*)data, 6)));
  }

} // namespace unnamed

void bind_peer_plugin()
{
    class_<
        peer_plugin_wrap, boost::shared_ptr<peer_plugin_wrap>, boost::noncopyable
    >("peer_plugin")
        .def(
            "add_handshake"
          , &peer_plugin::add_handshake, &peer_plugin_wrap::default_add_handshake
        )
        .def(
            "on_handshake"
          , &peer_plugin::on_handshake, &peer_plugin_wrap::default_on_handshake
        )
        .def(
            "on_extension_handshake"
          , &peer_plugin::on_extension_handshake
          , &peer_plugin_wrap::default_on_extension_handshake
        )
        .def(
            "on_choke"
          , &peer_plugin::on_choke, &peer_plugin_wrap::default_on_choke
        )
        .def(
            "on_unchoke"
          , &peer_plugin::on_unchoke, &peer_plugin_wrap::default_on_unchoke
        )
        .def(
            "on_interested"
          , &peer_plugin::on_interested, &peer_plugin_wrap::default_on_interested
        )
        .def(
            "on_not_interested"
          , &peer_plugin::on_not_interested, &peer_plugin_wrap::default_on_not_interested
        )
        .def(
            "on_have"
          , &peer_plugin::on_have, &peer_plugin_wrap::default_on_have
        )
        .def(
            "on_bitfield"
          , &peer_plugin::on_bitfield, &peer_plugin_wrap::default_on_bitfield
        )
        .def(
            "on_request"
          , &peer_plugin::on_request, &peer_plugin_wrap::default_on_request
        )
        .def(
            "on_piece"
          , &peer_plugin::on_piece, &peer_plugin_wrap::default_on_piece
        )
        .def(
            "on_cancel"
          , &peer_plugin::on_cancel, &peer_plugin_wrap::default_on_cancel
        )
        .def(
            "on_piece_pass"
          , &peer_plugin::on_piece_pass, &peer_plugin_wrap::default_on_piece_pass
        )
        .def(
            "on_piece_failed"
          , &peer_plugin::on_piece_failed, &peer_plugin_wrap::default_on_piece_failed
        )
        .def(
            "tick"
          , &peer_plugin::tick, &peer_plugin_wrap::default_tick
        )
        .def(
            "write_request"
          , &peer_plugin::write_request, &peer_plugin_wrap::default_write_request
        )
        // These seem to make VC7.1 freeze. Needs special handling.

        /*.def(
            "on_extended"
          , &peer_plugin::on_extended, &peer_plugin_wrap::default_on_extended
        )
        .def(
            "on_unknown_message"
          , &peer_plugin::on_unknown_message, &peer_plugin_wrap::default_on_unknown_message
        )*/
        ;

    def("get_buffer", &get_buffer);
}

<|endoftext|>


<commit_before>/*
 * An istream which duplicates data.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "istream_tee.hxx"
#include "istream_internal.hxx"
#include "util/Cast.hxx"

#include <assert.h>

struct TeeIstream {
    struct {
        struct istream istream;

        /**
         * A weak output is one which is closed automatically when all
         * "strong" outputs have been closed - it will not keep up the
         * istream_tee object alone.
         */
        bool weak;

        bool enabled;
    } outputs[2];

    struct istream *input;

    /**
     * These flags control whether istream_tee_close[12]() may restart
     * reading for the other output.
     */
    bool reading, in_data;

#ifndef NDEBUG
    bool closed_while_reading, closed_while_data;
#endif

    /**
     * The number of bytes to skip for output 0.  The first output has
     * already consumed this many bytes, but the second output
     * blocked.
     */
    size_t skip;
};

static GQuark
tee_quark(void)
{
    return g_quark_from_static_string("tee");
}

static size_t
tee_feed0(TeeIstream &tee, const char *data, size_t length)
{
    if (!tee.outputs[0].enabled)
        return length;

    if (length <= tee.skip)
        /* all of this has already been sent to the first input, but
           the second one didn't accept it yet */
        return length;

    /* skip the part which was already sent */
    data += tee.skip;
    length -= tee.skip;

    size_t nbytes = istream_invoke_data(&tee.outputs[0].istream,
                                        data, length);
    if (nbytes > 0) {
        tee.skip += nbytes;
        return tee.skip;
    }

    if (tee.outputs[0].enabled || !tee.outputs[1].enabled)
        /* first output is blocking, or both closed: give up */
        return 0;

    /* the first output has been closed inside the data() callback,
       but the second is still alive: continue with the second
       output */
    return length;
}

static size_t
tee_feed1(TeeIstream &tee, const void *data, size_t length)
{
    if (!tee.outputs[1].enabled)
        return length;

    size_t nbytes = istream_invoke_data(&tee.outputs[1].istream, data, length);
    if (nbytes == 0 && !tee.outputs[1].enabled &&
        tee.outputs[0].enabled)
        /* during the data callback, outputs[1] has been closed,
           but outputs[0] continues; instead of returning 0 here,
           use outputs[0]'s result */
        return length;

    return nbytes;
}

static size_t
tee_feed(TeeIstream &tee, const void *data, size_t length)
{
    size_t nbytes0 = tee_feed0(tee, (const char *)data, length);
    if (nbytes0 == 0)
        return 0;

    size_t nbytes1 = tee_feed1(tee, data, nbytes0);
    if (nbytes1 > 0 && tee.outputs[0].enabled) {
        assert(nbytes1 <= tee.skip);
        tee.skip -= nbytes1;
    }

    return nbytes1;
}

/*
 * istream handler
 *
 */

static size_t
tee_input_data(const void *data, size_t length, void *ctx)
{
    TeeIstream &tee = *(TeeIstream *)ctx;

    assert(!tee.in_data);

    pool_ref(tee.outputs[0].istream.pool);
    tee.in_data = true;

    size_t nbytes = tee_feed(tee, data, length);

    tee.in_data = false;
    pool_unref(tee.outputs[0].istream.pool);

    return nbytes;
}

static void
tee_input_eof(void *ctx)
{
    TeeIstream &tee = *(TeeIstream *)ctx;

    assert(tee.input != nullptr);

    pool_ref(tee.outputs[0].istream.pool);

    tee.input = nullptr;

    /* clean up in reverse order */

    if (tee.outputs[1].enabled) {
        tee.outputs[1].enabled = false;
        istream_deinit_eof(&tee.outputs[1].istream);
    }

    if (tee.outputs[0].enabled) {
        tee.outputs[0].enabled = false;
        istream_deinit_eof(&tee.outputs[0].istream);
    }

    pool_unref(tee.outputs[0].istream.pool);
}

static void
tee_input_abort(GError *error, void *ctx)
{
    TeeIstream &tee = *(TeeIstream *)ctx;

    assert(tee.input != nullptr);

    pool_ref(tee.outputs[0].istream.pool);

    tee.input = nullptr;

    /* clean up in reverse order */

    if (tee.outputs[1].enabled) {
        tee.outputs[1].enabled = false;
        istream_deinit_abort(&tee.outputs[1].istream, g_error_copy(error));
    }

    if (tee.outputs[0].enabled) {
        tee.outputs[0].enabled = false;
        istream_deinit_abort(&tee.outputs[0].istream, g_error_copy(error));
    }

    g_error_free(error);

    pool_unref(tee.outputs[0].istream.pool);
}

static const struct istream_handler tee_input_handler = {
    .data = tee_input_data,
    /* .direct = tee_input_direct, XXX implement that using sys_tee() */
    .eof = tee_input_eof,
    .abort = tee_input_abort,
};

/*
 * istream implementation 0
 *
 */

#ifdef __clang__
#pragma GCC diagnostic ignored "-Wextended-offsetof"
#endif

static inline TeeIstream &
istream_to_tee0(struct istream *istream)
{
    return *ContainerCast(istream, TeeIstream, outputs[0].istream);
}

static off_t
istream_tee_available0(struct istream *istream, bool partial)
{
    TeeIstream &tee = istream_to_tee0(istream);

    assert(tee.outputs[0].enabled);

    return istream_available(tee.input, partial);
}

static void
istream_tee_read0(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee0(istream);

    assert(tee.outputs[0].enabled);
    assert(!tee.reading);

    pool_ref(tee.outputs[0].istream.pool);

    tee.reading = true;
    istream_read(tee.input);
    tee.reading = false;

    pool_unref(tee.outputs[0].istream.pool);
}

static void
istream_tee_close0(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee0(istream);

    assert(tee.outputs[0].enabled);

    tee.outputs[0].enabled = false;

#ifndef NDEBUG
    if (tee.reading)
        tee.closed_while_reading = true;
    if (tee.in_data)
        tee.closed_while_data = true;
#endif

    if (tee.input != nullptr) {
        if (!tee.outputs[1].enabled)
            istream_free_handler(&tee.input);
        else if (tee.outputs[1].weak) {
            pool_ref(tee.outputs[0].istream.pool);

            istream_free_handler(&tee.input);

            if (tee.outputs[1].enabled) {
                tee.outputs[1].enabled = false;

                GError *error =
                    g_error_new_literal(tee_quark(), 0,
                                        "closing the weak second output");
                istream_deinit_abort(&tee.outputs[1].istream, error);
            }

            pool_unref(tee.outputs[0].istream.pool);
        }
    }

    if (tee.input != nullptr && tee.outputs[1].enabled &&
        istream_has_handler(&tee.outputs[1].istream) &&
        !tee.in_data && !tee.reading)
        istream_read(tee.input);

    istream_deinit(&tee.outputs[0].istream);
}

static const struct istream_class istream_tee0 = {
    .available = istream_tee_available0,
    .read = istream_tee_read0,
    .close = istream_tee_close0,
};

/*
 * istream implementation 2
 *
 */

static inline TeeIstream &
istream_to_tee1(struct istream *istream)
{
    return *ContainerCast(istream, TeeIstream, outputs[1].istream);
}

static off_t
istream_tee_available1(struct istream *istream, bool partial)
{
    TeeIstream &tee = istream_to_tee1(istream);

    assert(tee.outputs[1].enabled);

    return istream_available(tee.input, partial);
}

static void
istream_tee_read1(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee1(istream);

    assert(!tee.reading);

    pool_ref(tee.outputs[1].istream.pool);

    tee.reading = true;
    istream_read(tee.input);
    tee.reading = false;

    pool_unref(tee.outputs[1].istream.pool);
}

static void
istream_tee_close1(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee1(istream);

    assert(tee.outputs[1].enabled);

    tee.outputs[1].enabled = false;

#ifndef NDEBUG
    if (tee.reading)
        tee.closed_while_reading = true;
    if (tee.in_data)
        tee.closed_while_data = true;
#endif

    if (tee.input != nullptr) {
        if (!tee.outputs[0].enabled)
            istream_free_handler(&tee.input);
        else if (tee.outputs[0].weak) {
            pool_ref(tee.outputs[0].istream.pool);

            istream_free_handler(&tee.input);

            if (tee.outputs[0].enabled) {
                tee.outputs[0].enabled = false;

                GError *error =
                    g_error_new_literal(tee_quark(), 0,
                                        "closing the weak first output");
                istream_deinit_abort(&tee.outputs[0].istream, error);
            }

            pool_unref(tee.outputs[0].istream.pool);
        }
    }

    if (tee.input != nullptr && tee.outputs[0].enabled &&
        istream_has_handler(&tee.outputs[0].istream) &&
        !tee.in_data && !tee.reading)
        istream_read(tee.input);

    istream_deinit(&tee.outputs[1].istream);
}

static const struct istream_class istream_tee1 = {
    .available = istream_tee_available1,
    .read = istream_tee_read1,
    .close = istream_tee_close1,
};

/*
 * constructor
 *
 */

struct istream *
istream_tee_new(struct pool *pool, struct istream *input,
                bool first_weak, bool second_weak)
{
    TeeIstream *tee = (TeeIstream *)
        istream_new(pool, &istream_tee0, sizeof(*tee));

    assert(input != nullptr);
    assert(!istream_has_handler(input));

    istream_init(&tee->outputs[1].istream, &istream_tee1, tee->outputs[0].istream.pool);

    tee->outputs[0].weak = first_weak;
    tee->outputs[0].enabled = true;
    tee->outputs[1].weak = second_weak;
    tee->outputs[1].enabled = true;

    istream_assign_handler(&tee->input, input,
                           &tee_input_handler, tee,
                           0);

    tee->reading = false;
    tee->in_data = false;
    tee->skip = 0;

#ifndef NDEBUG
    tee->closed_while_reading = tee->closed_while_data = false;
#endif

    return &tee->outputs[0].istream;
}

struct istream *
istream_tee_second(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee0(istream);

    return &tee.outputs[1].istream;
}

<commit_msg>istream_tee: use NewFromPool() instead of istream_new()<commit_after>/*
 * An istream which duplicates data.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "istream_tee.hxx"
#include "istream_internal.hxx"
#include "pool.hxx"
#include "util/Cast.hxx"

#include <assert.h>

struct TeeIstream {
    struct {
        struct istream istream;

        /**
         * A weak output is one which is closed automatically when all
         * "strong" outputs have been closed - it will not keep up the
         * istream_tee object alone.
         */
        bool weak;

        bool enabled;
    } outputs[2];

    struct istream *input;

    /**
     * These flags control whether istream_tee_close[12]() may restart
     * reading for the other output.
     */
    bool reading, in_data;

#ifndef NDEBUG
    bool closed_while_reading, closed_while_data;
#endif

    /**
     * The number of bytes to skip for output 0.  The first output has
     * already consumed this many bytes, but the second output
     * blocked.
     */
    size_t skip;
};

static GQuark
tee_quark(void)
{
    return g_quark_from_static_string("tee");
}

static size_t
tee_feed0(TeeIstream &tee, const char *data, size_t length)
{
    if (!tee.outputs[0].enabled)
        return length;

    if (length <= tee.skip)
        /* all of this has already been sent to the first input, but
           the second one didn't accept it yet */
        return length;

    /* skip the part which was already sent */
    data += tee.skip;
    length -= tee.skip;

    size_t nbytes = istream_invoke_data(&tee.outputs[0].istream,
                                        data, length);
    if (nbytes > 0) {
        tee.skip += nbytes;
        return tee.skip;
    }

    if (tee.outputs[0].enabled || !tee.outputs[1].enabled)
        /* first output is blocking, or both closed: give up */
        return 0;

    /* the first output has been closed inside the data() callback,
       but the second is still alive: continue with the second
       output */
    return length;
}

static size_t
tee_feed1(TeeIstream &tee, const void *data, size_t length)
{
    if (!tee.outputs[1].enabled)
        return length;

    size_t nbytes = istream_invoke_data(&tee.outputs[1].istream, data, length);
    if (nbytes == 0 && !tee.outputs[1].enabled &&
        tee.outputs[0].enabled)
        /* during the data callback, outputs[1] has been closed,
           but outputs[0] continues; instead of returning 0 here,
           use outputs[0]'s result */
        return length;

    return nbytes;
}

static size_t
tee_feed(TeeIstream &tee, const void *data, size_t length)
{
    size_t nbytes0 = tee_feed0(tee, (const char *)data, length);
    if (nbytes0 == 0)
        return 0;

    size_t nbytes1 = tee_feed1(tee, data, nbytes0);
    if (nbytes1 > 0 && tee.outputs[0].enabled) {
        assert(nbytes1 <= tee.skip);
        tee.skip -= nbytes1;
    }

    return nbytes1;
}

/*
 * istream handler
 *
 */

static size_t
tee_input_data(const void *data, size_t length, void *ctx)
{
    TeeIstream &tee = *(TeeIstream *)ctx;

    assert(!tee.in_data);

    pool_ref(tee.outputs[0].istream.pool);
    tee.in_data = true;

    size_t nbytes = tee_feed(tee, data, length);

    tee.in_data = false;
    pool_unref(tee.outputs[0].istream.pool);

    return nbytes;
}

static void
tee_input_eof(void *ctx)
{
    TeeIstream &tee = *(TeeIstream *)ctx;

    assert(tee.input != nullptr);

    pool_ref(tee.outputs[0].istream.pool);

    tee.input = nullptr;

    /* clean up in reverse order */

    if (tee.outputs[1].enabled) {
        tee.outputs[1].enabled = false;
        istream_deinit_eof(&tee.outputs[1].istream);
    }

    if (tee.outputs[0].enabled) {
        tee.outputs[0].enabled = false;
        istream_deinit_eof(&tee.outputs[0].istream);
    }

    pool_unref(tee.outputs[0].istream.pool);
}

static void
tee_input_abort(GError *error, void *ctx)
{
    TeeIstream &tee = *(TeeIstream *)ctx;

    assert(tee.input != nullptr);

    pool_ref(tee.outputs[0].istream.pool);

    tee.input = nullptr;

    /* clean up in reverse order */

    if (tee.outputs[1].enabled) {
        tee.outputs[1].enabled = false;
        istream_deinit_abort(&tee.outputs[1].istream, g_error_copy(error));
    }

    if (tee.outputs[0].enabled) {
        tee.outputs[0].enabled = false;
        istream_deinit_abort(&tee.outputs[0].istream, g_error_copy(error));
    }

    g_error_free(error);

    pool_unref(tee.outputs[0].istream.pool);
}

static const struct istream_handler tee_input_handler = {
    .data = tee_input_data,
    /* .direct = tee_input_direct, XXX implement that using sys_tee() */
    .eof = tee_input_eof,
    .abort = tee_input_abort,
};

/*
 * istream implementation 0
 *
 */

#ifdef __clang__
#pragma GCC diagnostic ignored "-Wextended-offsetof"
#endif

static inline TeeIstream &
istream_to_tee0(struct istream *istream)
{
    return *ContainerCast(istream, TeeIstream, outputs[0].istream);
}

static off_t
istream_tee_available0(struct istream *istream, bool partial)
{
    TeeIstream &tee = istream_to_tee0(istream);

    assert(tee.outputs[0].enabled);

    return istream_available(tee.input, partial);
}

static void
istream_tee_read0(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee0(istream);

    assert(tee.outputs[0].enabled);
    assert(!tee.reading);

    pool_ref(tee.outputs[0].istream.pool);

    tee.reading = true;
    istream_read(tee.input);
    tee.reading = false;

    pool_unref(tee.outputs[0].istream.pool);
}

static void
istream_tee_close0(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee0(istream);

    assert(tee.outputs[0].enabled);

    tee.outputs[0].enabled = false;

#ifndef NDEBUG
    if (tee.reading)
        tee.closed_while_reading = true;
    if (tee.in_data)
        tee.closed_while_data = true;
#endif

    if (tee.input != nullptr) {
        if (!tee.outputs[1].enabled)
            istream_free_handler(&tee.input);
        else if (tee.outputs[1].weak) {
            pool_ref(tee.outputs[0].istream.pool);

            istream_free_handler(&tee.input);

            if (tee.outputs[1].enabled) {
                tee.outputs[1].enabled = false;

                GError *error =
                    g_error_new_literal(tee_quark(), 0,
                                        "closing the weak second output");
                istream_deinit_abort(&tee.outputs[1].istream, error);
            }

            pool_unref(tee.outputs[0].istream.pool);
        }
    }

    if (tee.input != nullptr && tee.outputs[1].enabled &&
        istream_has_handler(&tee.outputs[1].istream) &&
        !tee.in_data && !tee.reading)
        istream_read(tee.input);

    istream_deinit(&tee.outputs[0].istream);
}

static const struct istream_class istream_tee0 = {
    .available = istream_tee_available0,
    .read = istream_tee_read0,
    .close = istream_tee_close0,
};

/*
 * istream implementation 2
 *
 */

static inline TeeIstream &
istream_to_tee1(struct istream *istream)
{
    return *ContainerCast(istream, TeeIstream, outputs[1].istream);
}

static off_t
istream_tee_available1(struct istream *istream, bool partial)
{
    TeeIstream &tee = istream_to_tee1(istream);

    assert(tee.outputs[1].enabled);

    return istream_available(tee.input, partial);
}

static void
istream_tee_read1(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee1(istream);

    assert(!tee.reading);

    pool_ref(tee.outputs[1].istream.pool);

    tee.reading = true;
    istream_read(tee.input);
    tee.reading = false;

    pool_unref(tee.outputs[1].istream.pool);
}

static void
istream_tee_close1(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee1(istream);

    assert(tee.outputs[1].enabled);

    tee.outputs[1].enabled = false;

#ifndef NDEBUG
    if (tee.reading)
        tee.closed_while_reading = true;
    if (tee.in_data)
        tee.closed_while_data = true;
#endif

    if (tee.input != nullptr) {
        if (!tee.outputs[0].enabled)
            istream_free_handler(&tee.input);
        else if (tee.outputs[0].weak) {
            pool_ref(tee.outputs[0].istream.pool);

            istream_free_handler(&tee.input);

            if (tee.outputs[0].enabled) {
                tee.outputs[0].enabled = false;

                GError *error =
                    g_error_new_literal(tee_quark(), 0,
                                        "closing the weak first output");
                istream_deinit_abort(&tee.outputs[0].istream, error);
            }

            pool_unref(tee.outputs[0].istream.pool);
        }
    }

    if (tee.input != nullptr && tee.outputs[0].enabled &&
        istream_has_handler(&tee.outputs[0].istream) &&
        !tee.in_data && !tee.reading)
        istream_read(tee.input);

    istream_deinit(&tee.outputs[1].istream);
}

static const struct istream_class istream_tee1 = {
    .available = istream_tee_available1,
    .read = istream_tee_read1,
    .close = istream_tee_close1,
};

/*
 * constructor
 *
 */

struct istream *
istream_tee_new(struct pool *pool, struct istream *input,
                bool first_weak, bool second_weak)
{
    assert(input != nullptr);
    assert(!istream_has_handler(input));

    auto tee = NewFromPool<TeeIstream>(*pool);
    istream_init(&tee->outputs[0].istream, &istream_tee0, pool);
    istream_init(&tee->outputs[1].istream, &istream_tee1, pool);

    tee->outputs[0].weak = first_weak;
    tee->outputs[0].enabled = true;
    tee->outputs[1].weak = second_weak;
    tee->outputs[1].enabled = true;

    istream_assign_handler(&tee->input, input,
                           &tee_input_handler, tee,
                           0);

    tee->reading = false;
    tee->in_data = false;
    tee->skip = 0;

#ifndef NDEBUG
    tee->closed_while_reading = tee->closed_while_data = false;
#endif

    return &tee->outputs[0].istream;
}

struct istream *
istream_tee_second(struct istream *istream)
{
    TeeIstream &tee = istream_to_tee0(istream);

    return &tee.outputs[1].istream;
}

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2021, Jan de Visser <jan@finiandarcy.com>
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#include <iomanip>
#include <iostream>
#include <vector>

#include <core/Format.h>
#include <core/StringUtil.h>
#include <jv80/cpu/addressregister.h>
#include <jv80/cpu/controller.h>
#include <jv80/cpu/opcodes.h>
#include <jv80/cpu/register.h>
#include <jv80/cpu/registers.h>
#include <jv80/cpu/ComponentContainer.h>

namespace Obelix::JV80::CPU {

static MicroCode mcNMI = {
    .opcode = 0xFE,
    .instruction = "__nmi",
    .addressingMode = AddressingMode::Implied,
    .steps = {
        // Push the processor flags:
        { .action = MicroCode::XADDR, .src = SP, .target = MEMADDR, .opflags = SystemBus::INC },
        { .action = MicroCode::XADDR, .src = RHS, .target = MEM, .opflags = SystemBus::None },

        // Push the return address:
        { .action = MicroCode::XADDR, .src = SP, .target = MEMADDR, .opflags = SystemBus::INC },
        { .action = MicroCode::XDATA, .src = PC, .target = MEM, .opflags = SystemBus::None },
        { .action = MicroCode::XADDR, .src = SP, .target = MEMADDR, .opflags = SystemBus::INC },
        { .action = MicroCode::XDATA, .src = PC, .target = MEM, .opflags = SystemBus::MSB },

        // Load PC with the subroutine address:
        { .action = MicroCode::XADDR, .src = CONTROLLER, .target = PC, .opflags = SystemBus::Done },
    }
};

MicroCodeRunner::MicroCodeRunner(Controller& controller, SystemBus& bus, const MicroCode* microCode)
    : m_controller(controller)
    , m_bus(bus)
    , m_mc(microCode)
    , m_steps()
{

    evaluateCondition();
    fetchSteps();
    if (!(m_mc->addressingMode & AddressingMode::Done)) {
        int ix = -1;
        do {
            ix++;
            m_steps.emplace_back(m_mc->steps[ix]);
        } while (!(m_mc->steps[ix].opflags & SystemBus::Done));
    }
}

void MicroCodeRunner::evaluateCondition()
{
    byte valid_byte;
    switch (m_mc->condition_op) {
    case MicroCode::And:
        valid_byte = m_bus.isSet((SystemBus::ProcessorFlags)m_mc->condition);
        break;
    case MicroCode::Nand:
        valid_byte = !m_bus.isSet((SystemBus::ProcessorFlags)m_mc->condition);
        break;
    default:
        valid_byte = true;
        break;
    }
    m_valid = valid_byte != 0;
}

void MicroCodeRunner::fetchSteps()
{
    switch (m_mc->addressingMode & AddressingMode::Mask) {
    case AddressingMode::ImmediateByte:
        fetchImmediateByte();
        break;
    case AddressingMode::ImmediateWord:
        fetchImmediateWord();
        break;
    case AddressingMode::IndirectByte:
        fetchIndirectByte();
        break;
    case AddressingMode::IndirectWord:
        fetchIndirectWord();
        break;
    default:
        break;
    }
}

void MicroCodeRunner::fetchImmediateByte()
{
    byte target = (m_valid) ? m_mc->target : TX;
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, target, SystemBus::None });
}

void MicroCodeRunner::fetchImmediateWord()
{
    byte target = (m_valid && (m_mc->target != PC) && (m_mc->target != MEMADDR)) ? m_mc->target : TX;
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, target, SystemBus::None });
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, target, SystemBus::MSB });
    if (m_valid && m_mc->target != target) {
        m_steps.push_back({ MicroCode::Action::XADDR, TX, m_mc->target, SystemBus::None });
    }
}

void MicroCodeRunner::fetchIndirectByte()
{
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::None });
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::MSB });
    if (m_valid) {
        m_steps.push_back({ MicroCode::Action::XADDR, TX, MEMADDR, SystemBus::None });
        m_steps.push_back({ MicroCode::Action::XDATA, MEM, m_mc->target, SystemBus::None });
    }
}

void MicroCodeRunner::fetchIndirectWord()
{
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::None });
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::MSB });
    if (m_valid) {
        m_steps.push_back({ MicroCode::Action::XADDR, TX, MEMADDR, SystemBus::INC });
        m_steps.push_back({ MicroCode::Action::XDATA, MEM, m_mc->target, SystemBus::None });
        m_steps.push_back({ MicroCode::Action::XADDR, TX, MEMADDR, SystemBus::None });
        m_steps.push_back({ MicroCode::Action::XDATA, MEM, m_mc->target, SystemBus::MSB });
    }
}

bool MicroCodeRunner::grabConstant(int step)
{
    switch (m_mc->addressingMode & Mask) {
    case AddressingMode::Implied:
        m_complete = (step == 1);
        break;
    case AddressingMode::ImmediateByte:
    case AddressingMode::ImpliedByte:
        if (step == 2) {
            m_constant = m_bus.readDataBus();
            m_complete = true;
        }
        break;
    case AddressingMode::ImmediateWord:
    case AddressingMode::ImpliedWord:
    case AddressingMode::IndirectWord:
    case AddressingMode::IndirectByte:
        if (step == 2) {
            m_constant = m_bus.readDataBus();
        } else if (step == 4) {
            m_constant |= (((word)m_bus.readDataBus()) << 8);
            m_complete = true;
        }
        break;
    default:
        break;
    }
    return m_complete;
}

SystemError MicroCodeRunner::executeNextStep(int step)
{
    MicroCode::MicroCodeStep s = m_steps[step];
    byte src = (s.src != DEREFCONTROLLER) ? s.src : m_controller.scratch();
    byte target = (s.target != DEREFCONTROLLER) ? s.target : m_controller.scratch();
    switch (s.action) {
    case MicroCode::XDATA:
        m_bus.xdata(src, target, s.opflags & SystemBus::Mask);
        break;
    case MicroCode::XADDR:
        m_bus.xaddr(src, target, s.opflags & SystemBus::Mask);
        break;
    case MicroCode::IO:
        m_bus.io(src, target, s.opflags & SystemBus::Mask);
        break;
    case MicroCode::OTHER:
        switch (s.opflags & SystemBus::Mask) {
        case SystemBus::Halt:
            m_bus.stop();
            break;
        default:
            std::cerr << "Unhandled operation flag '" << std::hex << s.opflags
                      << "' for instruction " << std::hex << m_mc->opcode << " step "
                      << std::dec << step << std::endl;
            return SystemError { SystemErrorCode::InvalidMicroCode };
        }
        break;
    default:
        std::cerr << "Unhandled microcode action '" << std::hex << s.action
                  << "' for instruction " << std::hex << m_mc->opcode << " step "
                  << std::dec << step << std::endl;
        return SystemError { SystemErrorCode::InvalidMicroCode };
    }
    return {};
}

bool MicroCodeRunner::hasStep(int step)
{
    return m_steps.size() > step;
}

std::string MicroCodeRunner::instruction() const
{
    std::string instr = m_mc->instruction;
    bool do_format = false;
    auto pos = instr.find_first_of("$xxxx");
    if (pos != std::string::npos) {
        instr.replace(pos, 5, "{04x}", 5);
        do_format = true;
    } else {
        pos = instr.find_first_of("$xx");
        if (pos != std::string::npos) {
            instr.replace(pos, 3, "{02x}", 5);
            do_format = true;
        }
    }
    if (do_format)
        instr = format(instr, m_constant);
    return to_lower(instr);
}

word MicroCodeRunner::constant() const
{
    return m_constant;
}

// -----------------------------------------------------------------------

Controller::Controller(const MicroCode* mc)
    : Register(IR)
    , m_microCode(mc)
{
}

std::string Controller::instruction() const
{
    if (m_runner) {
        return m_runner->instruction();
    } else {
        return "----";
    }
}

word Controller::constant() const
{
    if (m_runner) {
        return m_runner->constant();
    } else {
        return 0;
    }
}

std::string Controller::to_string() const
{
    return format("{01x}. IR {02x} {04x} {>15s} Step {}", address(), getValue(), constant(), instruction().c_str(), m_step);
}

SystemError Controller::reset()
{
    TRY_RETURN(Register::reset());
    m_step = 0;
    return {};
}

SystemError Controller::onRisingClockEdge()
{
    if (bus()->getAddress() == CONTROLLER) {
        if (!bus()->xdata()) {
            bus()->putOnDataBus(scratch());
        } else if (!bus()->xaddr()) {
            bus()->putOnDataBus(m_interruptVector & 0x00FF);
            bus()->putOnAddrBus((m_interruptVector & 0xFF00) >> 8);
        }
    } else {
        TRY_RETURN(this->Register::onRisingClockEdge());
    }
    return {};
}

SystemError Controller::onHighClock()
{
    if (bus()->putAddress() == CONTROLLER) {
        if (!bus()->xdata()) {
            m_scratch = bus()->readDataBus();
        } else if (!bus()->xaddr()) {
            m_interruptVector = bus()->readDataBus();
            m_interruptVector |= ((word)bus()->readAddrBus()) << 8;
        }
    } else {
        TRY_RETURN(this->Register::onHighClock());
    }
    m_suspended++;
    if (m_runner && m_runner->grabConstant(m_step - 2)) {
        sendEvent(EV_VALUECHANGED);
    }
    return {};
}

SystemError Controller::onLowClock()
{
    const MicroCode* mc = nullptr;

    if ((m_suspended >= 1) && (runMode() == SystemBus::RunMode::BreakAtInstruction) && m_runner && m_runner->complete()) {
        m_suspended = -16;
        bus()->suspend();
        return {};
    }

    switch (m_step) {
    case 0:
        m_pc = bus()->backplane()->component<AddressRegister>(PC)->getValue();
        bus()->xaddr(PC, MEMADDR, SystemBus::INC);
        break;
    case 1:
        bus()->xdata(MEM, IR, SystemBus::None);
        m_suspended = 0;
        break;
    case 2:
        if (!bus()->nmi()) {
            if ((m_interruptVector != 0xFFFF) && !m_servicingNMI) {
                mc = &mcNMI;
                m_servicingNMI = true;
            }
            bus()->clearNmi();
        } else {
            mc = m_microCode + getValue();
            if (!mc->opcode) {
                m_runner.reset();
            } else if (mc->opcode != getValue()) {
                std::cerr << "Microcode mismatch for opcode " << std::hex << getValue()
                          << ": 'opcode' field contains " << std::hex << mc->opcode
                          << std::endl;
                return SystemError { SystemErrorCode::InvalidMicroCode };
            }
        }
        if (mc->opcode) {
            m_runner.emplace(*this, *bus(), mc);
        } else {
            m_runner.reset();
        }
        // fall through:
    default:
        if (m_runner.has_value() && m_runner.value().hasStep(m_step - 2)) {
            TRY_RETURN(m_runner->executeNextStep(m_step - 2));
            if (!bus()->halt()) {
                sendEvent(EV_AFTERINSTRUCTION);
            }
        } else {
            if (getValue() == RTI) {
                m_servicingNMI = false;
            }
            sendEvent(EV_AFTERINSTRUCTION);
            m_runner.reset();
            setValue(0);
            if (!bus()->nmi()) {
                m_step = 1;
            } else {
                m_step = 0;
                m_pc = bus()->backplane()->component<AddressRegister>(PC)->getValue();
                bus()->xaddr(PC, MEMADDR, SystemBus::INC);
            }
        }
        break;
    }
    m_step++;
    sendEvent(EV_STEPCHANGED);
    if (runMode() == SystemBus::RunMode::BreakAtClock) {
        bus()->suspend();
    }
    return {};
}

std::string Controller::instructionWithOpcode(int opcode) const
{
    auto mc = m_microCode + opcode;
    return (mc && (mc->opcode == opcode)) ? mc->instruction : "NOP";
}

int Controller::opcodeForInstruction(const std::string& instr) const
{
    for (int ix = 0; ix < 256; ix++) {
        auto mc = m_microCode + ix;
        if (mc && (mc->opcode == ix) && (instr == mc->instruction)) {
            return ix;
        }
    }
    return -1;
}
}
<commit_msg>Fixed instruction() method<commit_after>/*
 * Copyright (c) 2021, Jan de Visser <jan@finiandarcy.com>
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#include <iomanip>
#include <iostream>
#include <vector>

#include <core/Format.h>
#include <core/StringUtil.h>
#include <jv80/cpu/addressregister.h>
#include <jv80/cpu/controller.h>
#include <jv80/cpu/opcodes.h>
#include <jv80/cpu/register.h>
#include <jv80/cpu/registers.h>
#include <jv80/cpu/ComponentContainer.h>

namespace Obelix::JV80::CPU {

static MicroCode mcNMI = {
    .opcode = 0xFE,
    .instruction = "__nmi",
    .addressingMode = AddressingMode::Implied,
    .steps = {
        // Push the processor flags:
        { .action = MicroCode::XADDR, .src = SP, .target = MEMADDR, .opflags = SystemBus::INC },
        { .action = MicroCode::XADDR, .src = RHS, .target = MEM, .opflags = SystemBus::None },

        // Push the return address:
        { .action = MicroCode::XADDR, .src = SP, .target = MEMADDR, .opflags = SystemBus::INC },
        { .action = MicroCode::XDATA, .src = PC, .target = MEM, .opflags = SystemBus::None },
        { .action = MicroCode::XADDR, .src = SP, .target = MEMADDR, .opflags = SystemBus::INC },
        { .action = MicroCode::XDATA, .src = PC, .target = MEM, .opflags = SystemBus::MSB },

        // Load PC with the subroutine address:
        { .action = MicroCode::XADDR, .src = CONTROLLER, .target = PC, .opflags = SystemBus::Done },
    }
};

MicroCodeRunner::MicroCodeRunner(Controller& controller, SystemBus& bus, const MicroCode* microCode)
    : m_controller(controller)
    , m_bus(bus)
    , m_mc(microCode)
    , m_steps()
{

    evaluateCondition();
    fetchSteps();
    if (!(m_mc->addressingMode & AddressingMode::Done)) {
        int ix = -1;
        do {
            ix++;
            m_steps.emplace_back(m_mc->steps[ix]);
        } while (!(m_mc->steps[ix].opflags & SystemBus::Done));
    }
}

void MicroCodeRunner::evaluateCondition()
{
    byte valid_byte;
    switch (m_mc->condition_op) {
    case MicroCode::And:
        valid_byte = m_bus.isSet((SystemBus::ProcessorFlags)m_mc->condition);
        break;
    case MicroCode::Nand:
        valid_byte = !m_bus.isSet((SystemBus::ProcessorFlags)m_mc->condition);
        break;
    default:
        valid_byte = true;
        break;
    }
    m_valid = valid_byte != 0;
}

void MicroCodeRunner::fetchSteps()
{
    switch (m_mc->addressingMode & AddressingMode::Mask) {
    case AddressingMode::ImmediateByte:
        fetchImmediateByte();
        break;
    case AddressingMode::ImmediateWord:
        fetchImmediateWord();
        break;
    case AddressingMode::IndirectByte:
        fetchIndirectByte();
        break;
    case AddressingMode::IndirectWord:
        fetchIndirectWord();
        break;
    default:
        break;
    }
}

void MicroCodeRunner::fetchImmediateByte()
{
    byte target = (m_valid) ? m_mc->target : TX;
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, target, SystemBus::None });
}

void MicroCodeRunner::fetchImmediateWord()
{
    byte target = (m_valid && (m_mc->target != PC) && (m_mc->target != MEMADDR)) ? m_mc->target : TX;
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, target, SystemBus::None });
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, target, SystemBus::MSB });
    if (m_valid && m_mc->target != target) {
        m_steps.push_back({ MicroCode::Action::XADDR, TX, m_mc->target, SystemBus::None });
    }
}

void MicroCodeRunner::fetchIndirectByte()
{
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::None });
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::MSB });
    if (m_valid) {
        m_steps.push_back({ MicroCode::Action::XADDR, TX, MEMADDR, SystemBus::None });
        m_steps.push_back({ MicroCode::Action::XDATA, MEM, m_mc->target, SystemBus::None });
    }
}

void MicroCodeRunner::fetchIndirectWord()
{
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::None });
    m_steps.push_back({ MicroCode::Action::XADDR, PC, MEMADDR, SystemBus::INC });
    m_steps.push_back({ MicroCode::Action::XDATA, MEM, TX, SystemBus::MSB });
    if (m_valid) {
        m_steps.push_back({ MicroCode::Action::XADDR, TX, MEMADDR, SystemBus::INC });
        m_steps.push_back({ MicroCode::Action::XDATA, MEM, m_mc->target, SystemBus::None });
        m_steps.push_back({ MicroCode::Action::XADDR, TX, MEMADDR, SystemBus::None });
        m_steps.push_back({ MicroCode::Action::XDATA, MEM, m_mc->target, SystemBus::MSB });
    }
}

bool MicroCodeRunner::grabConstant(int step)
{
    switch (m_mc->addressingMode & Mask) {
    case AddressingMode::Implied:
        m_complete = (step == 1);
        break;
    case AddressingMode::ImmediateByte:
    case AddressingMode::ImpliedByte:
        if (step == 2) {
            m_constant = m_bus.readDataBus();
            m_complete = true;
        }
        break;
    case AddressingMode::ImmediateWord:
    case AddressingMode::ImpliedWord:
    case AddressingMode::IndirectWord:
    case AddressingMode::IndirectByte:
        if (step == 2) {
            m_constant = m_bus.readDataBus();
        } else if (step == 4) {
            m_constant |= (((word)m_bus.readDataBus()) << 8);
            m_complete = true;
        }
        break;
    default:
        break;
    }
    return m_complete;
}

SystemError MicroCodeRunner::executeNextStep(int step)
{
    MicroCode::MicroCodeStep s = m_steps[step];
    byte src = (s.src != DEREFCONTROLLER) ? s.src : m_controller.scratch();
    byte target = (s.target != DEREFCONTROLLER) ? s.target : m_controller.scratch();
    switch (s.action) {
    case MicroCode::XDATA:
        m_bus.xdata(src, target, s.opflags & SystemBus::Mask);
        break;
    case MicroCode::XADDR:
        m_bus.xaddr(src, target, s.opflags & SystemBus::Mask);
        break;
    case MicroCode::IO:
        m_bus.io(src, target, s.opflags & SystemBus::Mask);
        break;
    case MicroCode::OTHER:
        switch (s.opflags & SystemBus::Mask) {
        case SystemBus::Halt:
            m_bus.stop();
            break;
        default:
            std::cerr << "Unhandled operation flag '" << std::hex << s.opflags
                      << "' for instruction " << std::hex << m_mc->opcode << " step "
                      << std::dec << step << std::endl;
            return SystemError { SystemErrorCode::InvalidMicroCode };
        }
        break;
    default:
        std::cerr << "Unhandled microcode action '" << std::hex << s.action
                  << "' for instruction " << std::hex << m_mc->opcode << " step "
                  << std::dec << step << std::endl;
        return SystemError { SystemErrorCode::InvalidMicroCode };
    }
    return {};
}

bool MicroCodeRunner::hasStep(int step)
{
    return m_steps.size() > step;
}

std::string MicroCodeRunner::instruction() const
{
    std::string instr = m_mc->instruction;
    bool do_format = false;
    auto pos = instr.find("$xxxx");
    if (pos != std::string::npos) {
        instr.replace(pos, 5, "${04x}", 6);
        do_format = true;
    } else {
        pos = instr.find_first_of("$xx");
        if (pos != std::string::npos) {
            instr.replace(pos, 3, "${02x}", 6);
            do_format = true;
        }
    }
    if (do_format)
        instr = format(instr, m_constant);
    return to_lower(instr);
}

word MicroCodeRunner::constant() const
{
    return m_constant;
}

// -----------------------------------------------------------------------

Controller::Controller(const MicroCode* mc)
    : Register(IR)
    , m_microCode(mc)
{
}

std::string Controller::instruction() const
{
    if (m_runner) {
        return m_runner->instruction();
    } else {
        return "----";
    }
}

word Controller::constant() const
{
    if (m_runner) {
        return m_runner->constant();
    } else {
        return 0;
    }
}

std::string Controller::to_string() const
{
    return format("{01x}. IR {02x} {04x} {>15s} Step {}", address(), getValue(), constant(), instruction().c_str(), m_step);
}

SystemError Controller::reset()
{
    TRY_RETURN(Register::reset());
    m_step = 0;
    return {};
}

SystemError Controller::onRisingClockEdge()
{
    if (bus()->getAddress() == CONTROLLER) {
        if (!bus()->xdata()) {
            bus()->putOnDataBus(scratch());
        } else if (!bus()->xaddr()) {
            bus()->putOnDataBus(m_interruptVector & 0x00FF);
            bus()->putOnAddrBus((m_interruptVector & 0xFF00) >> 8);
        }
    } else {
        TRY_RETURN(this->Register::onRisingClockEdge());
    }
    return {};
}

SystemError Controller::onHighClock()
{
    if (bus()->putAddress() == CONTROLLER) {
        if (!bus()->xdata()) {
            m_scratch = bus()->readDataBus();
        } else if (!bus()->xaddr()) {
            m_interruptVector = bus()->readDataBus();
            m_interruptVector |= ((word)bus()->readAddrBus()) << 8;
        }
    } else {
        TRY_RETURN(this->Register::onHighClock());
    }
    m_suspended++;
    if (m_runner && m_runner->grabConstant(m_step - 2)) {
        sendEvent(EV_VALUECHANGED);
    }
    return {};
}

SystemError Controller::onLowClock()
{
    const MicroCode* mc = nullptr;

    if ((m_suspended >= 1) && (runMode() == SystemBus::RunMode::BreakAtInstruction) && m_runner && m_runner->complete()) {
        m_suspended = -16;
        bus()->suspend();
        return {};
    }

    switch (m_step) {
    case 0:
        m_pc = bus()->backplane()->component<AddressRegister>(PC)->getValue();
        bus()->xaddr(PC, MEMADDR, SystemBus::INC);
        break;
    case 1:
        bus()->xdata(MEM, IR, SystemBus::None);
        m_suspended = 0;
        break;
    case 2:
        if (!bus()->nmi()) {
            if ((m_interruptVector != 0xFFFF) && !m_servicingNMI) {
                mc = &mcNMI;
                m_servicingNMI = true;
            }
            bus()->clearNmi();
        } else {
            mc = m_microCode + getValue();
            if (!mc->opcode) {
                m_runner.reset();
            } else if (mc->opcode != getValue()) {
                std::cerr << "Microcode mismatch for opcode " << std::hex << getValue()
                          << ": 'opcode' field contains " << std::hex << mc->opcode
                          << std::endl;
                return SystemError { SystemErrorCode::InvalidMicroCode };
            }
        }
        if (mc->opcode) {
            m_runner.emplace(*this, *bus(), mc);
        } else {
            m_runner.reset();
        }
        // fall through:
    default:
        if (m_runner.has_value() && m_runner.value().hasStep(m_step - 2)) {
            TRY_RETURN(m_runner->executeNextStep(m_step - 2));
            if (!bus()->halt()) {
                sendEvent(EV_AFTERINSTRUCTION);
            }
        } else {
            if (getValue() == RTI) {
                m_servicingNMI = false;
            }
            sendEvent(EV_AFTERINSTRUCTION);
            m_runner.reset();
            setValue(0);
            if (!bus()->nmi()) {
                m_step = 1;
            } else {
                m_step = 0;
                m_pc = bus()->backplane()->component<AddressRegister>(PC)->getValue();
                bus()->xaddr(PC, MEMADDR, SystemBus::INC);
            }
        }
        break;
    }
    m_step++;
    sendEvent(EV_STEPCHANGED);
    if (runMode() == SystemBus::RunMode::BreakAtClock) {
        bus()->suspend();
    }
    return {};
}

std::string Controller::instructionWithOpcode(int opcode) const
{
    auto mc = m_microCode + opcode;
    return (mc && (mc->opcode == opcode)) ? mc->instruction : "NOP";
}

int Controller::opcodeForInstruction(const std::string& instr) const
{
    for (int ix = 0; ix < 256; ix++) {
        auto mc = m_microCode + ix;
        if (mc && (mc->opcode == ix) && (instr == mc->instruction)) {
            return ix;
        }
    }
    return -1;
}

}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file battery.cpp
 *
 * Library calls for battery functionality.
 *
 * @author Julian Oes <julian@oes.ch>
 * @author Timothy Scott <timothy@auterion.com>
 */

#include "battery.h"
#include <mathlib/mathlib.h>
#include <cstring>
#include <px4_platform_common/defines.h>

using namespace time_literals;

Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) :
	ModuleParams(parent),
	_index(index < 1 || index > 9 ? 1 : index)
{
	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
	_current_filter_a.setParameters(expected_filter_dt, .5f);
	_throttle_filter.setParameters(expected_filter_dt, 1.f);

	if (index > 9 || index < 1) {
		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
	}

	// 16 chars for parameter name + null terminator
	char param_name[17];

	snprintf(param_name, sizeof(param_name), "BAT%d_V_EMPTY", _index);
	_param_handles.v_empty = param_find(param_name);

	if (_param_handles.v_empty == PARAM_INVALID) {
		PX4_ERR("Could not find parameter with name %s", param_name);
	}

	snprintf(param_name, sizeof(param_name), "BAT%d_V_CHARGED", _index);
	_param_handles.v_charged = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_N_CELLS", _index);
	_param_handles.n_cells = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
	_param_handles.capacity = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
	_param_handles.v_load_drop = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
	_param_handles.r_internal = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", _index);
	_param_handles.source = param_find(param_name);

	_param_handles.low_thr = param_find("BAT_LOW_THR");
	_param_handles.crit_thr = param_find("BAT_CRIT_THR");
	_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");

	_param_handles.v_empty_old = param_find("BAT_V_EMPTY");
	_param_handles.v_charged_old = param_find("BAT_V_CHARGED");
	_param_handles.n_cells_old = param_find("BAT_N_CELLS");
	_param_handles.capacity_old = param_find("BAT_CAPACITY");
	_param_handles.v_load_drop_old = param_find("BAT_V_LOAD_DROP");
	_param_handles.r_internal_old = param_find("BAT_R_INTERNAL");
	_param_handles.source_old = param_find("BAT_SOURCE");

	updateParams();
}

void Battery::reset()
{
	memset(&_battery_status, 0, sizeof(_battery_status));
	_battery_status.current_a = -1.f;
	_battery_status.remaining = 1.f;
	_battery_status.scale = 1.f;
	_battery_status.cell_count = _params.n_cells;
	// TODO: check if it is sane to reset warning to NONE
	_battery_status.warning = battery_status_s::BATTERY_WARNING_NONE;
	_battery_status.connected = false;
	_battery_status.capacity = _params.capacity;
	_battery_status.temperature = NAN;
	_battery_status.id = (uint8_t) _index;
}

void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
				  int source, int priority, float throttle_normalized)
{
	reset();

	if (!_battery_initialized) {
		_voltage_filter_v.reset(voltage_v);
		_current_filter_a.reset(current_a);
		_throttle_filter.reset(throttle_normalized);
	}

	_voltage_filter_v.update(voltage_v);
	_current_filter_a.update(current_a);
	_throttle_filter.update(throttle_normalized);
	sumDischarged(timestamp, current_a);
	estimateRemaining(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
	computeScale();

	if (_battery_initialized) {
		determineWarning(connected);
	}

	if (_voltage_filter_v.getState() > 2.1f) {
		_battery_initialized = true;
		_battery_status.voltage_v = voltage_v;
		_battery_status.voltage_filtered_v = _voltage_filter_v.getState();
		_battery_status.scale = _scale;
		_battery_status.current_a = current_a;
		_battery_status.current_filtered_a = _current_filter_a.getState();
		_battery_status.discharged_mah = _discharged_mah;
		_battery_status.warning = _warning;
		_battery_status.remaining = _remaining;
		_battery_status.connected = connected;
		_battery_status.source = source;
		_battery_status.priority = priority;

		static constexpr int uorb_max_cells = sizeof(_battery_status.voltage_cell_v) / sizeof(
				_battery_status.voltage_cell_v[0]);

		// Fill cell voltages with average values to work around BATTERY_STATUS message not allowing to report just total voltage
		for (int i = 0; (i < _battery_status.cell_count) && (i < uorb_max_cells); i++) {
			_battery_status.voltage_cell_v[i] = _battery_status.voltage_filtered_v / _battery_status.cell_count;
		}
	}

	if (source == _params.source) {
		publish();
	}
}

void Battery::publish()
{
	_battery_status.timestamp = hrt_absolute_time();
	_battery_status_pub.publish(_battery_status);
}

void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
{
	// Not a valid measurement
	if (current_a < 0.f) {
		// Because the measurement was invalid we need to stop integration
		// and re-initialize with the next valid measurement
		_last_timestamp = 0;
		return;
	}

	// Ignore first update because we don't know dt.
	if (_last_timestamp != 0) {
		const float dt = (timestamp - _last_timestamp) / 1e6;
		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
		_discharged_mah += _discharged_mah_loop;
	}

	_last_timestamp = timestamp;
}

void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
{
	// remaining battery capacity based on voltage
	float cell_voltage = voltage_v / _params.n_cells;

	// correct battery voltage locally for load drop to avoid estimation fluctuations
	if (_params.r_internal >= 0.f) {
		cell_voltage += _params.r_internal * current_a;

	} else {
		// assume linear relation between throttle and voltage drop
		cell_voltage += throttle * _params.v_load_drop;
	}

	_remaining_voltage = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);

	// choose which quantity we're using for final reporting
	if (_params.capacity > 0.f) {
		// if battery capacity is known, fuse voltage measurement with used capacity
		if (!_battery_initialized) {
			// initialization of the estimation state
			_remaining = _remaining_voltage;

		} else {
			// The lower the voltage the more adjust the estimate with it to avoid deep discharge
			const float weight_v = 3e-4f * (1 - _remaining_voltage);
			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
			// directly apply current capacity slope calculated using current
			_remaining -= _discharged_mah_loop / _params.capacity;
			_remaining = math::max(_remaining, 0.f);
		}

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

void Battery::determineWarning(bool connected)
{
	if (connected) {
		// propagate warning state only if the state is higher, otherwise remain in current warning state
		if (_remaining < _params.emergen_thr) {
			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

		} else if (_remaining < _params.crit_thr) {
			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;

		} else if (_remaining < _params.low_thr) {
			_warning = battery_status_s::BATTERY_WARNING_LOW;

		} else {
			_warning = battery_status_s::BATTERY_WARNING_NONE;
		}
	}
}

void Battery::computeScale()
{
	const float voltage_range = (_params.v_charged - _params.v_empty);

	// reusing capacity calculation to get single cell voltage before drop
	const float bat_v = _params.v_empty + (voltage_range * _remaining_voltage);

	_scale = _params.v_charged / bat_v;

	if (_scale > 1.3f) { // Allow at most 30% compensation
		_scale = 1.3f;

	} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
		_scale = 1.f;
	}
}

void Battery::updateParams()
{
	if (_index == 1) {
		migrateParam<float>(_param_handles.v_empty_old, _param_handles.v_empty, &_params.v_empty_old, &_params.v_empty,
				    _first_parameter_update);
		migrateParam<float>(_param_handles.v_charged_old, _param_handles.v_charged, &_params.v_charged_old, &_params.v_charged,
				    _first_parameter_update);
		migrateParam<int>(_param_handles.n_cells_old, _param_handles.n_cells, &_params.n_cells_old, &_params.n_cells,
				  _first_parameter_update);
		migrateParam<float>(_param_handles.capacity_old, _param_handles.capacity, &_params.capacity_old, &_params.capacity,
				    _first_parameter_update);
		migrateParam<float>(_param_handles.v_load_drop_old, _param_handles.v_load_drop, &_params.v_load_drop_old,
				    &_params.v_load_drop, _first_parameter_update);
		migrateParam<float>(_param_handles.r_internal_old, _param_handles.r_internal, &_params.r_internal_old,
				    &_params.r_internal, _first_parameter_update);
		migrateParam<int>(_param_handles.source_old, _param_handles.source, &_params.source_old, &_params.source,
				  _first_parameter_update);

	} else {
		param_get(_param_handles.v_empty, &_params.v_empty);
		param_get(_param_handles.v_charged, &_params.v_charged);
		param_get(_param_handles.n_cells, &_params.n_cells);
		param_get(_param_handles.capacity, &_params.capacity);
		param_get(_param_handles.v_load_drop, &_params.v_load_drop);
		param_get(_param_handles.r_internal, &_params.r_internal);
		param_get(_param_handles.source, &_params.source);
	}

	param_get(_param_handles.low_thr, &_params.low_thr);
	param_get(_param_handles.crit_thr, &_params.crit_thr);
	param_get(_param_handles.emergen_thr, &_params.emergen_thr);

	ModuleParams::updateParams();

	_first_parameter_update = false;
}
<commit_msg>battery: don't init uint16_t with -1.0f<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file battery.cpp
 *
 * Library calls for battery functionality.
 *
 * @author Julian Oes <julian@oes.ch>
 * @author Timothy Scott <timothy@auterion.com>
 */

#include "battery.h"
#include <mathlib/mathlib.h>
#include <cstring>
#include <px4_platform_common/defines.h>

using namespace time_literals;

Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) :
	ModuleParams(parent),
	_index(index < 1 || index > 9 ? 1 : index)
{
	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
	_current_filter_a.setParameters(expected_filter_dt, .5f);
	_throttle_filter.setParameters(expected_filter_dt, 1.f);

	if (index > 9 || index < 1) {
		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
	}

	// 16 chars for parameter name + null terminator
	char param_name[17];

	snprintf(param_name, sizeof(param_name), "BAT%d_V_EMPTY", _index);
	_param_handles.v_empty = param_find(param_name);

	if (_param_handles.v_empty == PARAM_INVALID) {
		PX4_ERR("Could not find parameter with name %s", param_name);
	}

	snprintf(param_name, sizeof(param_name), "BAT%d_V_CHARGED", _index);
	_param_handles.v_charged = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_N_CELLS", _index);
	_param_handles.n_cells = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
	_param_handles.capacity = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
	_param_handles.v_load_drop = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
	_param_handles.r_internal = param_find(param_name);

	snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", _index);
	_param_handles.source = param_find(param_name);

	_param_handles.low_thr = param_find("BAT_LOW_THR");
	_param_handles.crit_thr = param_find("BAT_CRIT_THR");
	_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");

	_param_handles.v_empty_old = param_find("BAT_V_EMPTY");
	_param_handles.v_charged_old = param_find("BAT_V_CHARGED");
	_param_handles.n_cells_old = param_find("BAT_N_CELLS");
	_param_handles.capacity_old = param_find("BAT_CAPACITY");
	_param_handles.v_load_drop_old = param_find("BAT_V_LOAD_DROP");
	_param_handles.r_internal_old = param_find("BAT_R_INTERNAL");
	_param_handles.source_old = param_find("BAT_SOURCE");

	updateParams();
}

void Battery::reset()
{
	memset(&_battery_status, 0, sizeof(_battery_status));
	_battery_status.current_a = -1.f;
	_battery_status.remaining = 1.f;
	_battery_status.scale = 1.f;
	_battery_status.cell_count = _params.n_cells;
	// TODO: check if it is sane to reset warning to NONE
	_battery_status.warning = battery_status_s::BATTERY_WARNING_NONE;
	_battery_status.connected = false;
	_battery_status.capacity = _params.capacity > 0.0f ? (uint16_t)_params.capacity : 0;
	_battery_status.temperature = NAN;
	_battery_status.id = (uint8_t) _index;
}

void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
				  int source, int priority, float throttle_normalized)
{
	reset();

	if (!_battery_initialized) {
		_voltage_filter_v.reset(voltage_v);
		_current_filter_a.reset(current_a);
		_throttle_filter.reset(throttle_normalized);
	}

	_voltage_filter_v.update(voltage_v);
	_current_filter_a.update(current_a);
	_throttle_filter.update(throttle_normalized);
	sumDischarged(timestamp, current_a);
	estimateRemaining(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
	computeScale();

	if (_battery_initialized) {
		determineWarning(connected);
	}

	if (_voltage_filter_v.getState() > 2.1f) {
		_battery_initialized = true;
		_battery_status.voltage_v = voltage_v;
		_battery_status.voltage_filtered_v = _voltage_filter_v.getState();
		_battery_status.scale = _scale;
		_battery_status.current_a = current_a;
		_battery_status.current_filtered_a = _current_filter_a.getState();
		_battery_status.discharged_mah = _discharged_mah;
		_battery_status.warning = _warning;
		_battery_status.remaining = _remaining;
		_battery_status.connected = connected;
		_battery_status.source = source;
		_battery_status.priority = priority;

		static constexpr int uorb_max_cells = sizeof(_battery_status.voltage_cell_v) / sizeof(
				_battery_status.voltage_cell_v[0]);

		// Fill cell voltages with average values to work around BATTERY_STATUS message not allowing to report just total voltage
		for (int i = 0; (i < _battery_status.cell_count) && (i < uorb_max_cells); i++) {
			_battery_status.voltage_cell_v[i] = _battery_status.voltage_filtered_v / _battery_status.cell_count;
		}
	}

	if (source == _params.source) {
		publish();
	}
}

void Battery::publish()
{
	_battery_status.timestamp = hrt_absolute_time();
	_battery_status_pub.publish(_battery_status);
}

void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
{
	// Not a valid measurement
	if (current_a < 0.f) {
		// Because the measurement was invalid we need to stop integration
		// and re-initialize with the next valid measurement
		_last_timestamp = 0;
		return;
	}

	// Ignore first update because we don't know dt.
	if (_last_timestamp != 0) {
		const float dt = (timestamp - _last_timestamp) / 1e6;
		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
		_discharged_mah += _discharged_mah_loop;
	}

	_last_timestamp = timestamp;
}

void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
{
	// remaining battery capacity based on voltage
	float cell_voltage = voltage_v / _params.n_cells;

	// correct battery voltage locally for load drop to avoid estimation fluctuations
	if (_params.r_internal >= 0.f) {
		cell_voltage += _params.r_internal * current_a;

	} else {
		// assume linear relation between throttle and voltage drop
		cell_voltage += throttle * _params.v_load_drop;
	}

	_remaining_voltage = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);

	// choose which quantity we're using for final reporting
	if (_params.capacity > 0.f) {
		// if battery capacity is known, fuse voltage measurement with used capacity
		if (!_battery_initialized) {
			// initialization of the estimation state
			_remaining = _remaining_voltage;

		} else {
			// The lower the voltage the more adjust the estimate with it to avoid deep discharge
			const float weight_v = 3e-4f * (1 - _remaining_voltage);
			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
			// directly apply current capacity slope calculated using current
			_remaining -= _discharged_mah_loop / _params.capacity;
			_remaining = math::max(_remaining, 0.f);
		}

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

void Battery::determineWarning(bool connected)
{
	if (connected) {
		// propagate warning state only if the state is higher, otherwise remain in current warning state
		if (_remaining < _params.emergen_thr) {
			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

		} else if (_remaining < _params.crit_thr) {
			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;

		} else if (_remaining < _params.low_thr) {
			_warning = battery_status_s::BATTERY_WARNING_LOW;

		} else {
			_warning = battery_status_s::BATTERY_WARNING_NONE;
		}
	}
}

void Battery::computeScale()
{
	const float voltage_range = (_params.v_charged - _params.v_empty);

	// reusing capacity calculation to get single cell voltage before drop
	const float bat_v = _params.v_empty + (voltage_range * _remaining_voltage);

	_scale = _params.v_charged / bat_v;

	if (_scale > 1.3f) { // Allow at most 30% compensation
		_scale = 1.3f;

	} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
		_scale = 1.f;
	}
}

void Battery::updateParams()
{
	if (_index == 1) {
		migrateParam<float>(_param_handles.v_empty_old, _param_handles.v_empty, &_params.v_empty_old, &_params.v_empty,
				    _first_parameter_update);
		migrateParam<float>(_param_handles.v_charged_old, _param_handles.v_charged, &_params.v_charged_old, &_params.v_charged,
				    _first_parameter_update);
		migrateParam<int>(_param_handles.n_cells_old, _param_handles.n_cells, &_params.n_cells_old, &_params.n_cells,
				  _first_parameter_update);
		migrateParam<float>(_param_handles.capacity_old, _param_handles.capacity, &_params.capacity_old, &_params.capacity,
				    _first_parameter_update);
		migrateParam<float>(_param_handles.v_load_drop_old, _param_handles.v_load_drop, &_params.v_load_drop_old,
				    &_params.v_load_drop, _first_parameter_update);
		migrateParam<float>(_param_handles.r_internal_old, _param_handles.r_internal, &_params.r_internal_old,
				    &_params.r_internal, _first_parameter_update);
		migrateParam<int>(_param_handles.source_old, _param_handles.source, &_params.source_old, &_params.source,
				  _first_parameter_update);

	} else {
		param_get(_param_handles.v_empty, &_params.v_empty);
		param_get(_param_handles.v_charged, &_params.v_charged);
		param_get(_param_handles.n_cells, &_params.n_cells);
		param_get(_param_handles.capacity, &_params.capacity);
		param_get(_param_handles.v_load_drop, &_params.v_load_drop);
		param_get(_param_handles.r_internal, &_params.r_internal);
		param_get(_param_handles.source, &_params.source);
	}

	param_get(_param_handles.low_thr, &_params.low_thr);
	param_get(_param_handles.crit_thr, &_params.crit_thr);
	param_get(_param_handles.emergen_thr, &_params.emergen_thr);

	ModuleParams::updateParams();

	_first_parameter_update = false;
}
<|endoftext|>


<commit_before>//
// This file is part of the Marble Desktop Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2007   Andrew Manson   <g.real.ate@gmail.com>
//

#include "GpsTracking.h"
#include "TrackPoint.h"
#include "Track.h"
#include "TrackSegment.h"
#include "AbstractLayer/AbstractLayer.h"

#include <QDebug>

GpsTracking::GpsTracking( Track *track, TrackingMethod method)
{
    Q_UNUSED( method );

    m_gpsCurrentPosition  = new TrackPoint( 0,0 );
    m_gpsPreviousPosition = new TrackPoint( 0,0 );
    m_gpsTracking         = new TrackPoint( 0,0 );

    m_gpsTrack    = track;
    m_gpsTrackSeg = 0;

#ifdef HAVE_LIBGPS
    m_gpsd     = new gpsmm();
    m_gpsdData = m_gpsd->open( "127.0.0.1", "2947" );

#endif
}

GpsTracking::~GpsTracking()
{
}

void GpsTracking::construct( const QSize &canvasSize, double radius,
                             Quaternion invRotAxis )
{
    QPointF position;
    QPointF previousPosition;

    bool draw = false;

    draw = m_gpsCurrentPosition -> getPixelPos( canvasSize,
            invRotAxis,
            (int)radius, &position );

    draw = m_gpsPreviousPosition -> getPixelPos( canvasSize,
            invRotAxis,
            (int)radius,
             &previousPosition );

    qDebug() << "positions " << position << previousPosition ;
    if ( !draw ) {
        return;
    }

    double distance = sqrt( AbstractLayer::distance( position,
                                                  previousPosition) );
    if (distance == 0) {
        return;
    }

    QPointF unitVector = ( position - previousPosition )
                                    / distance;
    // The normal of the unit vector between first and second
    QPointF unitVector2 = QPointF ( -unitVector.y(), unitVector.x());

    qDebug() <<"unit vectors" << unitVector << unitVector2;

    previousDraw = currentDraw;

    currentDraw.clear();
    currentDraw << position
                << ( position - ( unitVector * 9 )
                              + ( unitVector2 * 9 ) )
                << ( position + ( unitVector * 19.0 ) )
                << ( position - ( unitVector * 9 )
                              - ( unitVector2 * 9 ) );
}

QRegion GpsTracking::update(const QSize &canvasSize, double radius,
                            Quaternion invRotAxis)
{
// FIXME: doesn't compile in line 109
/*
#ifndef HAVE_LIBGPS
    Q_UNUSED( canvasSize );
    Q_UNUSED( radius );
    Q_UNUSED( invRotAxis );
#else

    if ( m_gpsdData != 0 ){
        m_gpsdData =m_gpsd->query( "p" );

        m_gpsTracking ->setPosition( m_gpsdData->fix.latitude,
                                     m_gpsdData->fix.longitude );

        if (m_gpsTrackSeg == 0 ){
            m_gpsTrackSeg = new TrackSegment();
        }
        if ( m_gpsPreviousPosition->position() != m_gpsTracking->position() ) {
            m_gpsTrackSeg->append( m_gpsPreviousPosition );
            m_gpsPreviousPosition = m_gpsCurrentPosition;
            m_gpsCurrentPosition  = new TrackPoint( *m_gpsTracking );
        }
    } else {
        if ( m_gpsTrackSeg != 0  && m_gpsTrackSeg->size() > 0 ) {
            m_gpsTrack->append( m_gpsTrackSeg );
            m_gpsTrackSeg = 0;
        }
    }

    construct( canvasSize, radius, invRotAxis );

    QRect temp1(currentDraw.boundingRect().toRect());
    QRect temp2(previousDraw.boundingRect().toRect());

    temp1.adjust( -5, -5, 10, 10);
    temp2.adjust( -5, -5, 10, 10);

    return QRegion(temp1).united( QRegion(temp2) );

#endif
    return QRegion();
*/
}

void GpsTracking::draw( ClipPainter *painter,
                        const QSize &canvasSize, double radius,
                        Quaternion invRotAxis )
{
    painter->setPen( Qt::black );
    painter->setBrush( Qt::white );
    painter->drawPolygon( currentDraw, Qt::OddEvenFill );

    if ( m_gpsTrackSeg != 0) {
       m_gpsTrackSeg->draw( painter, canvasSize, radius, invRotAxis );
    }
}

<commit_msg>at least return something ...<commit_after>//
// This file is part of the Marble Desktop Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2007   Andrew Manson   <g.real.ate@gmail.com>
//

#include "GpsTracking.h"
#include "TrackPoint.h"
#include "Track.h"
#include "TrackSegment.h"
#include "AbstractLayer/AbstractLayer.h"

#include <QDebug>

GpsTracking::GpsTracking( Track *track, TrackingMethod method)
{
    Q_UNUSED( method );

    m_gpsCurrentPosition  = new TrackPoint( 0,0 );
    m_gpsPreviousPosition = new TrackPoint( 0,0 );
    m_gpsTracking         = new TrackPoint( 0,0 );

    m_gpsTrack    = track;
    m_gpsTrackSeg = 0;

#ifdef HAVE_LIBGPS
    m_gpsd     = new gpsmm();
    m_gpsdData = m_gpsd->open( "127.0.0.1", "2947" );

#endif
}

GpsTracking::~GpsTracking()
{
}

void GpsTracking::construct( const QSize &canvasSize, double radius,
                             Quaternion invRotAxis )
{
    QPointF position;
    QPointF previousPosition;

    bool draw = false;

    draw = m_gpsCurrentPosition -> getPixelPos( canvasSize,
            invRotAxis,
            (int)radius, &position );

    draw = m_gpsPreviousPosition -> getPixelPos( canvasSize,
            invRotAxis,
            (int)radius,
             &previousPosition );

    qDebug() << "positions " << position << previousPosition ;
    if ( !draw ) {
        return;
    }

    double distance = sqrt( AbstractLayer::distance( position,
                                                  previousPosition) );
    if (distance == 0) {
        return;
    }

    QPointF unitVector = ( position - previousPosition )
                                    / distance;
    // The normal of the unit vector between first and second
    QPointF unitVector2 = QPointF ( -unitVector.y(), unitVector.x());

    qDebug() <<"unit vectors" << unitVector << unitVector2;

    previousDraw = currentDraw;

    currentDraw.clear();
    currentDraw << position
                << ( position - ( unitVector * 9 )
                              + ( unitVector2 * 9 ) )
                << ( position + ( unitVector * 19.0 ) )
                << ( position - ( unitVector * 9 )
                              - ( unitVector2 * 9 ) );
}

QRegion GpsTracking::update(const QSize &canvasSize, double radius,
                            Quaternion invRotAxis)
{
// FIXME: doesn't compile in line 109
/*
#ifndef HAVE_LIBGPS
    Q_UNUSED( canvasSize );
    Q_UNUSED( radius );
    Q_UNUSED( invRotAxis );
#else

    if ( m_gpsdData != 0 ){
        m_gpsdData =m_gpsd->query( "p" );

        m_gpsTracking ->setPosition( m_gpsdData->fix.latitude,
                                     m_gpsdData->fix.longitude );

        if (m_gpsTrackSeg == 0 ){
            m_gpsTrackSeg = new TrackSegment();
        }
        if ( m_gpsPreviousPosition->position() != m_gpsTracking->position() ) {
            m_gpsTrackSeg->append( m_gpsPreviousPosition );
            m_gpsPreviousPosition = m_gpsCurrentPosition;
            m_gpsCurrentPosition  = new TrackPoint( *m_gpsTracking );
        }
    } else {
        if ( m_gpsTrackSeg != 0  && m_gpsTrackSeg->size() > 0 ) {
            m_gpsTrack->append( m_gpsTrackSeg );
            m_gpsTrackSeg = 0;
        }
    }

    construct( canvasSize, radius, invRotAxis );

    QRect temp1(currentDraw.boundingRect().toRect());
    QRect temp2(previousDraw.boundingRect().toRect());

    temp1.adjust( -5, -5, 10, 10);
    temp2.adjust( -5, -5, 10, 10);

    return QRegion(temp1).united( QRegion(temp2) );

#endif
*/
    return QRegion();

}

void GpsTracking::draw( ClipPainter *painter,
                        const QSize &canvasSize, double radius,
                        Quaternion invRotAxis )
{
    painter->setPen( Qt::black );
    painter->setBrush( Qt::white );
    painter->drawPolygon( currentDraw, Qt::OddEvenFill );

    if ( m_gpsTrackSeg != 0) {
       m_gpsTrackSeg->draw( painter, canvasSize, radius, invRotAxis );
    }
}

<|endoftext|>