Quelle  vivid-cec.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-only
/*
 * vivid-cec.c - A Virtual Video Test Driver, cec emulation
 *
 * Copyright 2016 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
 */

#include <linux/delay.h>
#include <media/cec.h>

#include "vivid-core.h"
#include "vivid-cec.h"

#define CEC_START_BIT_US  4500
#define CEC_DATA_BIT_US   2400
#define CEC_MARGIN_US   350

struct xfer_on_bus {
 struct cec_adapter *adap;
 u8   status;
};

static bool find_dest_adap(struct vivid_dev *dev,
      struct cec_adapter *adap, u8 dest)
{
 unsigned int i, j;

 if (dest >= 0xf)
  return false;

 if (adap != dev->cec_rx_adap && dev->cec_rx_adap &&
     dev->cec_rx_adap->is_configured &&
     cec_has_log_addr(dev->cec_rx_adap, dest))
  return true;

 for (i = 0, j = 0; i < dev->num_inputs; i++) {
  unsigned int menu_idx =
   dev->input_is_connected_to_output[i];

  if (dev->input_type[i] != HDMI)
   continue;
  j++;
  if (menu_idx < FIXED_MENU_ITEMS)
   continue;

  struct vivid_dev *dev_tx = vivid_ctrl_hdmi_to_output_instance[menu_idx];
  unsigned int output = vivid_ctrl_hdmi_to_output_index[menu_idx];

  if (!dev_tx)
   continue;

  unsigned int hdmi_output = dev_tx->output_to_iface_index[output];

  if (adap == dev_tx->cec_tx_adap[hdmi_output])
   continue;
  if (!dev_tx->cec_tx_adap[hdmi_output]->is_configured)
   continue;
  if (cec_has_log_addr(dev_tx->cec_tx_adap[hdmi_output], dest))
   return true;
 }
 return false;
}

static bool xfer_ready(struct vivid_dev *dev)
{
 unsigned int i;
 bool ready = false;

 spin_lock(&dev->cec_xfers_slock);
 for (i = 0; i < ARRAY_SIZE(dev->xfers); i++) {
  if (dev->xfers[i].sft &&
      dev->xfers[i].sft <= dev->cec_sft) {
   ready = true;
   break;
  }
 }
 spin_unlock(&dev->cec_xfers_slock);

 return ready;
}

/*
 * If an adapter tries to send successive messages, it must wait for the
 * longest signal-free time between its transmissions. But, if another
 * adapter sends a message in the interim, then the wait can be reduced
 * because the messages are no longer successive. Make these adjustments
 * if necessary. Should be called holding cec_xfers_slock.
 */
static void adjust_sfts(struct vivid_dev *dev)
{
 unsigned int i;
 u8 initiator;

 for (i = 0; i < ARRAY_SIZE(dev->xfers); i++) {
  if (dev->xfers[i].sft <= CEC_SIGNAL_FREE_TIME_RETRY)
   continue;
  initiator = dev->xfers[i].msg[0] >> 4;
  if (initiator == dev->last_initiator)
   dev->xfers[i].sft = CEC_SIGNAL_FREE_TIME_NEXT_XFER;
  else
   dev->xfers[i].sft = CEC_SIGNAL_FREE_TIME_NEW_INITIATOR;
 }
}

/*
 * The main emulation of the bus on which CEC adapters attempt to send
 * messages to each other. The bus keeps track of how long it has been
 * signal-free and accepts a pending transmission only if the state of
 * the bus matches the transmission's signal-free requirements. It calls
 * cec_transmit_attempt_done() for all transmits that enter the bus and
 * cec_received_msg() for successful transmits.
 */
int vivid_cec_bus_thread(void *_dev)
{
 u32 last_sft;
 unsigned int i, j;
 unsigned int dest;
 ktime_t start, end;
 s64 delta_us, retry_us;
 struct vivid_dev *dev = _dev;

 dev->cec_sft = CEC_SIGNAL_FREE_TIME_NEXT_XFER;
 for (;;) {
  bool first = true;
  int wait_xfer_us = 0;
  bool valid_dest = false;
  int wait_arb_lost_us = 0;
  unsigned int first_idx = 0;
  unsigned int first_status = 0;
  struct cec_msg first_msg = {};
  struct xfer_on_bus xfers_on_bus[MAX_OUTPUTS] = {};

  wait_event_interruptible(dev->kthread_waitq_cec, xfer_ready(dev) ||
      kthread_should_stop());
  if (kthread_should_stop())
   break;
  last_sft = dev->cec_sft;
  dev->cec_sft = 0;
  /*
 * Move the messages that are ready onto the bus. The adapter with
 * the most leading zeros will win control of the bus and any other
 * adapters will lose arbitration.
 */
  spin_lock(&dev->cec_xfers_slock);
  for (i = 0; i < ARRAY_SIZE(dev->xfers); i++) {
   if (!dev->xfers[i].sft || dev->xfers[i].sft > last_sft)
    continue;
   if (first) {
    first = false;
    first_idx = i;
    xfers_on_bus[first_idx].adap = dev->xfers[i].adap;
    memcpy(first_msg.msg, dev->xfers[i].msg, dev->xfers[i].len);
    first_msg.len = dev->xfers[i].len;
   } else {
    xfers_on_bus[i].adap = dev->xfers[i].adap;
    xfers_on_bus[i].status = CEC_TX_STATUS_ARB_LOST;
    /*
 * For simplicity wait for all 4 bits of the initiator's
 * address even though HDMI specification uses bit-level
 * precision.
 */
    wait_arb_lost_us = 4 * CEC_DATA_BIT_US + CEC_START_BIT_US;
   }
   dev->xfers[i].sft = 0;
  }
  dev->last_initiator = cec_msg_initiator(&first_msg);
  adjust_sfts(dev);
  spin_unlock(&dev->cec_xfers_slock);

  dest = cec_msg_destination(&first_msg);
  valid_dest = cec_msg_is_broadcast(&first_msg);
  if (!valid_dest)
   valid_dest = find_dest_adap(dev, xfers_on_bus[first_idx].adap, dest);
  if (valid_dest) {
   first_status = CEC_TX_STATUS_OK;
   /*
 * Message length is in bytes, but each byte is transmitted in
 * a block of 10 bits.
 */
   wait_xfer_us = first_msg.len * 10 * CEC_DATA_BIT_US;
  } else {
   first_status = CEC_TX_STATUS_NACK;
   /*
 * A message that is not acknowledged stops transmitting after
 * the header block of 10 bits.
 */
   wait_xfer_us = 10 * CEC_DATA_BIT_US;
  }
  wait_xfer_us += CEC_START_BIT_US;
  xfers_on_bus[first_idx].status = first_status;

  /* Sleep as if sending messages on a real hardware bus. */
  start = ktime_get();
  if (wait_arb_lost_us) {
   usleep_range(wait_arb_lost_us - CEC_MARGIN_US, wait_arb_lost_us);
   for (i = 0; i < ARRAY_SIZE(xfers_on_bus); i++) {
    if (xfers_on_bus[i].status != CEC_TX_STATUS_ARB_LOST)
     continue;
    cec_transmit_attempt_done(xfers_on_bus[i].adap,
         CEC_TX_STATUS_ARB_LOST);
   }
   if (kthread_should_stop())
    break;
  }
  wait_xfer_us -= wait_arb_lost_us;
  usleep_range(wait_xfer_us - CEC_MARGIN_US, wait_xfer_us);
  cec_transmit_attempt_done(xfers_on_bus[first_idx].adap, first_status);
  if (kthread_should_stop())
   break;
  if (first_status == CEC_TX_STATUS_OK) {
   if (xfers_on_bus[first_idx].adap != dev->cec_rx_adap)
    cec_received_msg(dev->cec_rx_adap, &first_msg);
   for (i = 0, j = 0; i < dev->num_inputs; i++) {
    unsigned int menu_idx =
     dev->input_is_connected_to_output[i];

    if (dev->input_type[i] != HDMI)
     continue;
    j++;
    if (menu_idx < FIXED_MENU_ITEMS)
     continue;

    struct vivid_dev *dev_tx = vivid_ctrl_hdmi_to_output_instance[menu_idx];
    unsigned int output = vivid_ctrl_hdmi_to_output_index[menu_idx];

    if (!dev_tx)
     continue;

    unsigned int hdmi_output = dev_tx->output_to_iface_index[output];

    if (xfers_on_bus[first_idx].adap != dev_tx->cec_tx_adap[hdmi_output])
     cec_received_msg(dev_tx->cec_tx_adap[hdmi_output], &first_msg);
   }
  }
  end = ktime_get();
  /*
 * If the emulated transfer took more or less time than it should
 * have, then compensate by adjusting the wait time needed for the
 * bus to be signal-free for 3 bit periods (the retry time).
 */
  delta_us = div_s64(end - start, 1000);
  delta_us -= wait_xfer_us + wait_arb_lost_us;
  retry_us = CEC_SIGNAL_FREE_TIME_RETRY * CEC_DATA_BIT_US - delta_us;
  if (retry_us > CEC_MARGIN_US)
   usleep_range(retry_us - CEC_MARGIN_US, retry_us);
  dev->cec_sft = CEC_SIGNAL_FREE_TIME_RETRY;
  /*
 * If there are no messages that need to be retried, check if any
 * adapters that did not just transmit a message are ready to
 * transmit. If none of these adapters are ready, then increase
 * the signal-free time so that the bus is available to all
 * adapters and go back to waiting for a transmission.
 */
  while (dev->cec_sft >= CEC_SIGNAL_FREE_TIME_RETRY &&
         dev->cec_sft < CEC_SIGNAL_FREE_TIME_NEXT_XFER &&
         !xfer_ready(dev) && !kthread_should_stop()) {
   usleep_range(2 * CEC_DATA_BIT_US - CEC_MARGIN_US,
         2 * CEC_DATA_BIT_US);
   dev->cec_sft += 2;
  }
 }
 return 0;
}

static int vivid_cec_adap_enable(struct cec_adapter *adap, bool enable)
{
 adap->cec_pin_is_high = true;
 return 0;
}

static int vivid_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
{
 return 0;
}

static int vivid_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
       u32 signal_free_time, struct cec_msg *msg)
{
 struct vivid_dev *dev = cec_get_drvdata(adap);
 struct vivid_dev *dev_rx = dev;
 u8 idx = cec_msg_initiator(msg);
 u8 output = 0;

 if (dev->cec_rx_adap != adap) {
  int i;

  for (i = 0; i < dev->num_hdmi_outputs; i++)
   if (dev->cec_tx_adap[i] == adap)
    break;
  if (i == dev->num_hdmi_outputs)
   return -ENONET;
  output = dev->hdmi_index_to_output_index[i];
  dev_rx = dev->output_to_input_instance[output];
  if (!dev_rx)
   return -ENONET;
 }
 spin_lock(&dev_rx->cec_xfers_slock);
 dev_rx->xfers[idx].adap = adap;
 memcpy(dev_rx->xfers[idx].msg, msg->msg, CEC_MAX_MSG_SIZE);
 dev_rx->xfers[idx].len = msg->len;
 dev_rx->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_RETRY;
 if (signal_free_time > CEC_SIGNAL_FREE_TIME_RETRY) {
  if (idx == dev_rx->last_initiator)
   dev_rx->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_NEXT_XFER;
  else
   dev_rx->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_NEW_INITIATOR;
 }
 spin_unlock(&dev_rx->cec_xfers_slock);
 wake_up_interruptible(&dev_rx->kthread_waitq_cec);

 return 0;
}

static int vivid_received(struct cec_adapter *adap, struct cec_msg *msg)
{
 struct vivid_dev *dev = cec_get_drvdata(adap);
 struct cec_msg reply;
 u8 dest = cec_msg_destination(msg);

 if (cec_msg_is_broadcast(msg))
  dest = adap->log_addrs.log_addr[0];
 cec_msg_init(&reply, dest, cec_msg_initiator(msg));

 switch (cec_msg_opcode(msg)) {
 case CEC_MSG_SET_OSD_STRING: {
  u8 disp_ctl;
  char osd[14];

  if (!cec_is_sink(adap))
   break;
  cec_ops_set_osd_string(msg, &disp_ctl, osd);
  switch (disp_ctl) {
  case CEC_OP_DISP_CTL_DEFAULT:
   strscpy(dev->osd, osd, sizeof(dev->osd));
   dev->osd_jiffies = jiffies;
   break;
  case CEC_OP_DISP_CTL_UNTIL_CLEARED:
   strscpy(dev->osd, osd, sizeof(dev->osd));
   dev->osd_jiffies = 0;
   break;
  case CEC_OP_DISP_CTL_CLEAR:
   dev->osd[0] = 0;
   dev->osd_jiffies = 0;
   break;
  default:
   cec_msg_feature_abort(&reply, cec_msg_opcode(msg),
           CEC_OP_ABORT_INVALID_OP);
   cec_transmit_msg(adap, &reply, false);
   break;
  }
  return 0;
 }
 case CEC_MSG_VENDOR_COMMAND_WITH_ID: {
  u32 vendor_id;
  u8 size;
  const u8 *vendor_cmd;

  /*
 * If we receive <Vendor Command With ID> with our vendor ID
 * and with a payload of size 1, and the payload value is odd,
 * then we reply with the same message, but with the payload
 * byte incremented by 1.
 *
 * If the size is 1 and the payload value is even, then we
 * ignore the message.
 *
 * The reason we reply to odd instead of even payload values
 * is that it allows for testing of the corner case where the
 * reply value is 0 (0xff + 1 % 256).
 *
 * For other sizes we Feature Abort.
 *
 * This is added for the specific purpose of testing the
 * CEC_MSG_FL_REPLY_VENDOR_ID flag using vivid.
 */
  cec_ops_vendor_command_with_id(msg, &vendor_id, &size, &vendor_cmd);
  if (vendor_id != adap->log_addrs.vendor_id)
   break;
  if (size == 1) {
   // Ignore even op values
   if (!(vendor_cmd[0] & 1))
    return 0;
   reply.len = msg->len;
   memcpy(reply.msg + 1, msg->msg + 1, msg->len - 1);
   reply.msg[msg->len - 1]++;
  } else {
   cec_msg_feature_abort(&reply, cec_msg_opcode(msg),
           CEC_OP_ABORT_INVALID_OP);
  }
  cec_transmit_msg(adap, &reply, false);
  return 0;
 }
 }
 return -ENOMSG;
}

static const struct cec_adap_ops vivid_cec_adap_ops = {
 .adap_enable = vivid_cec_adap_enable,
 .adap_log_addr = vivid_cec_adap_log_addr,
 .adap_transmit = vivid_cec_adap_transmit,
 .received = vivid_received,
};

struct cec_adapter *vivid_cec_alloc_adap(struct vivid_dev *dev,
      unsigned int idx,
      bool is_source)
{
 u32 caps = CEC_CAP_DEFAULTS | CEC_CAP_MONITOR_ALL | CEC_CAP_MONITOR_PIN;
 char name[32];

 snprintf(name, sizeof(name), "vivid-%03d-vid-%s%d",
   dev->inst, is_source ? "out" : "cap", idx);
 return cec_allocate_adapter(&vivid_cec_adap_ops, dev,
        name, caps, CEC_MAX_LOG_ADDRS);
}