#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/netdevice.h>
#include <net/genetlink.h>
#include "dpll_core.h"
#include "dpll_netlink.h"
#include "dpll_nl.h"
#include <uapi/linux/dpll.h>
#define ASSERT_NOT_NULL(ptr) (WARN_ON(!ptr))
#define xa_for_each_marked_start(xa, index, entry, filter, start) \
for (index = start, entry = xa_find(xa, &index, ULONG_MAX, filter); \
entry; entry = xa_find_after(xa, &index, ULONG_MAX, filter))
struct dpll_dump_ctx {
unsigned long idx;
};
static struct dpll_dump_ctx *dpll_dump_context(struct netlink_callback *cb)
{
return (struct dpll_dump_ctx *)cb->ctx;
}
static int
dpll_msg_add_dev_handle(struct sk_buff *msg, struct dpll_device *dpll)
{
if (nla_put_u32(msg, DPLL_A_ID, dpll->id))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_dev_parent_handle(struct sk_buff *msg, u32 id)
{
if (nla_put_u32(msg, DPLL_A_PIN_PARENT_ID, id))
return -EMSGSIZE;
return 0;
}
static bool dpll_pin_available(struct dpll_pin *pin)
{
struct dpll_pin_ref *par_ref;
unsigned long i;
if (!xa_get_mark(&dpll_pin_xa, pin->id, DPLL_REGISTERED))
return false;
xa_for_each(&pin->parent_refs, i, par_ref)
if (xa_get_mark(&dpll_pin_xa, par_ref->pin->id,
DPLL_REGISTERED))
return true;
xa_for_each(&pin->dpll_refs, i, par_ref)
if (xa_get_mark(&dpll_device_xa, par_ref->dpll->id,
DPLL_REGISTERED))
return true;
return false;
}
static int dpll_msg_add_pin_handle(struct sk_buff *msg, struct dpll_pin *pin)
{
if (!pin)
return 0;
if (nla_put_u32(msg, DPLL_A_PIN_ID, pin->id))
return -EMSGSIZE;
return 0;
}
static struct dpll_pin *dpll_netdev_pin(const struct net_device *dev)
{
return rcu_dereference_rtnl(dev->dpll_pin);
}
int dpll_netdev_add_pin_handle(struct sk_buff *msg,
const struct net_device *dev)
{
return dpll_msg_add_pin_handle(msg, dpll_netdev_pin(dev));
}
static int
dpll_msg_add_mode(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_mode mode;
int ret;
ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_MODE, mode))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_mode_supported(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 };
enum dpll_mode mode;
int ret;
if (ops->supported_modes_get) {
ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes,
extack);
if (ret)
return ret;
} else {
ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack);
if (ret)
return ret;
__set_bit(mode, modes);
}
for_each_set_bit(mode, modes, DPLL_MODE_MAX + 1)
if (nla_put_u32(msg, DPLL_A_MODE_SUPPORTED, mode))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_phase_offset_monitor(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_feature_state state;
int ret;
if (ops->phase_offset_monitor_set && ops->phase_offset_monitor_get) {
ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll),
&state, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_PHASE_OFFSET_MONITOR, state))
return -EMSGSIZE;
}
return 0;
}
static int
dpll_msg_add_freq_monitor(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_feature_state state;
int ret;
if (ops->freq_monitor_set && ops->freq_monitor_get) {
ret = ops->freq_monitor_get(dpll, dpll_priv(dpll),
&state, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_FREQUENCY_MONITOR, state))
return -EMSGSIZE;
}
return 0;
}
static int
dpll_msg_add_phase_offset_avg_factor(struct sk_buff *msg,
struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
u32 factor;
int ret;
if (ops->phase_offset_avg_factor_get) {
ret = ops->phase_offset_avg_factor_get(dpll, dpll_priv(dpll),
&factor, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_PHASE_OFFSET_AVG_FACTOR, factor))
return -EMSGSIZE;
}
return 0;
}
static int
dpll_msg_add_lock_status(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_lock_status_error status_error = 0;
enum dpll_lock_status status;
int ret;
ret = ops->lock_status_get(dpll, dpll_priv(dpll), &status,
&status_error, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_LOCK_STATUS, status))
return -EMSGSIZE;
if (status_error &&
(status == DPLL_LOCK_STATUS_UNLOCKED ||
status == DPLL_LOCK_STATUS_HOLDOVER) &&
nla_put_u32(msg, DPLL_A_LOCK_STATUS_ERROR, status_error))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_temp(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
s32 temp;
int ret;
if (!ops->temp_get)
return 0;
ret = ops->temp_get(dpll, dpll_priv(dpll), &temp, extack);
if (ret)
return ret;
if (nla_put_s32(msg, DPLL_A_TEMP, temp))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_clock_quality_level(struct sk_buff *msg, struct dpll_device *dpll,
struct netlink_ext_ack *extack)
{
DECLARE_BITMAP(qls, DPLL_CLOCK_QUALITY_LEVEL_MAX + 1) = { 0 };
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_clock_quality_level ql;
int ret;
if (!ops->clock_quality_level_get)
return 0;
ret = ops->clock_quality_level_get(dpll, dpll_priv(dpll), qls, extack);
if (ret)
return ret;
for_each_set_bit(ql, qls, DPLL_CLOCK_QUALITY_LEVEL_MAX + 1)
if (nla_put_u32(msg, DPLL_A_CLOCK_QUALITY_LEVEL, ql))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_pin_prio(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
u32 prio;
int ret;
if (!ops->prio_get)
return 0;
ret = ops->prio_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), &prio, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_PIN_PRIO, prio))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_pin_on_dpll_state(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
enum dpll_pin_state state;
int ret;
if (!ops->state_on_dpll_get)
return 0;
ret = ops->state_on_dpll_get(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), &state, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_PIN_STATE, state))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_pin_operstate(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
enum dpll_pin_operstate operstate;
int ret;
if (!ops->operstate_on_dpll_get)
return 0;
ret = ops->operstate_on_dpll_get(pin,
dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll),
&operstate, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_PIN_OPERSTATE, operstate))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_pin_direction(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
enum dpll_pin_direction direction;
int ret;
ret = ops->direction_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), &direction, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_PIN_DIRECTION, direction))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_pin_phase_adjust(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
s32 phase_adjust;
int ret;
if (!ops->phase_adjust_get)
return 0;
ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll),
&phase_adjust, extack);
if (ret)
return ret;
if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST, phase_adjust))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_phase_offset(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
s64 phase_offset;
int ret;
if (!ops->phase_offset_get)
return 0;
ret = ops->phase_offset_get(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), &phase_offset,
extack);
if (ret)
return ret;
if (nla_put_64bit(msg, DPLL_A_PIN_PHASE_OFFSET, sizeof(phase_offset),
&phase_offset, DPLL_A_PIN_PAD))
return -EMSGSIZE;
return 0;
}
static int dpll_msg_add_ffo(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
enum dpll_ffo_type type,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_ffo_param ffo = { .type = type };
int ret;
if (!ops->ffo_get || !(ops->supported_ffo & BIT(type)))
return 0;
ret = ops->ffo_get(pin, dpll_pin_on_dpll_priv(ref->dpll, pin),
ref->dpll, dpll_priv(ref->dpll), &ffo, extack);
if (ret) {
if (ret == -ENODATA)
return 0;
return ret;
}
if (nla_put_sint(msg, DPLL_A_PIN_FRACTIONAL_FREQUENCY_OFFSET,
div_s64(ffo.ffo, 1000000)))
return -EMSGSIZE;
return nla_put_sint(msg,
DPLL_A_PIN_FRACTIONAL_FREQUENCY_OFFSET_PPT,
ffo.ffo);
}
static int dpll_msg_add_measured_freq(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *dev_ops = dpll_device_ops(ref->dpll);
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
enum dpll_feature_state state;
u64 measured_freq;
int ret;
if (!ops->measured_freq_get)
return 0;
ret = dev_ops->freq_monitor_get(dpll, dpll_priv(dpll),
&state, extack);
if (ret)
return ret;
if (state == DPLL_FEATURE_STATE_DISABLE)
return 0;
ret = ops->measured_freq_get(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), &measured_freq,
extack);
if (ret)
return ret;
if (nla_put_64bit(msg, DPLL_A_PIN_MEASURED_FREQUENCY,
sizeof(measured_freq), &measured_freq,
DPLL_A_PIN_PAD))
return -EMSGSIZE;
return 0;
}
static int
dpll_msg_add_pin_freq(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref, struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
struct nlattr *nest;
int fs, ret;
u64 freq;
if (!ops->frequency_get)
return 0;
ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), &freq, extack);
if (ret)
return ret;
if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY, sizeof(freq), &freq,
DPLL_A_PIN_PAD))
return -EMSGSIZE;
for (fs = 0; fs < pin->prop.freq_supported_num; fs++) {
nest = nla_nest_start(msg, DPLL_A_PIN_FREQUENCY_SUPPORTED);
if (!nest)
return -EMSGSIZE;
freq = pin->prop.freq_supported[fs].min;
if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MIN, sizeof(freq),
&freq, DPLL_A_PIN_PAD)) {
nla_nest_cancel(msg, nest);
return -EMSGSIZE;
}
freq = pin->prop.freq_supported[fs].max;
if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MAX, sizeof(freq),
&freq, DPLL_A_PIN_PAD)) {
nla_nest_cancel(msg, nest);
return -EMSGSIZE;
}
nla_nest_end(msg, nest);
}
return 0;
}
static int
dpll_msg_add_pin_esync(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref, struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
struct dpll_pin_esync esync;
struct nlattr *nest;
int ret, i;
if (!ops->esync_get)
return 0;
ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), &esync, extack);
if (ret == -EOPNOTSUPP)
return 0;
else if (ret)
return ret;
if (nla_put_64bit(msg, DPLL_A_PIN_ESYNC_FREQUENCY, sizeof(esync.freq),
&esync.freq, DPLL_A_PIN_PAD))
return -EMSGSIZE;
if (nla_put_u32(msg, DPLL_A_PIN_ESYNC_PULSE, esync.pulse))
return -EMSGSIZE;
for (i = 0; i < esync.range_num; i++) {
nest = nla_nest_start(msg,
DPLL_A_PIN_ESYNC_FREQUENCY_SUPPORTED);
if (!nest)
return -EMSGSIZE;
if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MIN,
sizeof(esync.range[i].min),
&esync.range[i].min, DPLL_A_PIN_PAD))
goto nest_cancel;
if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MAX,
sizeof(esync.range[i].max),
&esync.range[i].max, DPLL_A_PIN_PAD))
goto nest_cancel;
nla_nest_end(msg, nest);
}
return 0;
nest_cancel:
nla_nest_cancel(msg, nest);
return -EMSGSIZE;
}
static int
dpll_msg_add_pin_ref_sync(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *ref,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
struct dpll_device *dpll = ref->dpll;
void *pin_priv, *ref_sync_pin_priv;
struct dpll_pin *ref_sync_pin;
enum dpll_pin_state state;
struct nlattr *nest;
unsigned long index;
int ret;
pin_priv = dpll_pin_on_dpll_priv(dpll, pin);
xa_for_each(&pin->ref_sync_pins, index, ref_sync_pin) {
if (!dpll_pin_available(ref_sync_pin))
continue;
ref_sync_pin_priv = dpll_pin_on_dpll_priv(dpll, ref_sync_pin);
if (!ref_sync_pin_priv)
continue;
if (WARN_ON(!ops->ref_sync_get))
return -EOPNOTSUPP;
ret = ops->ref_sync_get(pin, pin_priv, ref_sync_pin,
ref_sync_pin_priv, &state, extack);
if (ret)
return ret;
nest = nla_nest_start(msg, DPLL_A_PIN_REFERENCE_SYNC);
if (!nest)
return -EMSGSIZE;
if (nla_put_s32(msg, DPLL_A_PIN_ID, ref_sync_pin->id))
goto nest_cancel;
if (nla_put_s32(msg, DPLL_A_PIN_STATE, state))
goto nest_cancel;
nla_nest_end(msg, nest);
}
return 0;
nest_cancel:
nla_nest_cancel(msg, nest);
return -EMSGSIZE;
}
static bool dpll_pin_is_freq_supported(struct dpll_pin *pin, u32 freq)
{
int fs;
for (fs = 0; fs < pin->prop.freq_supported_num; fs++)
if (freq >= pin->prop.freq_supported[fs].min &&
freq <= pin->prop.freq_supported[fs].max)
return true;
return false;
}
static int
dpll_msg_add_pin_parents(struct sk_buff *msg, struct dpll_pin *pin,
struct dpll_pin_ref *dpll_ref,
struct netlink_ext_ack *extack)
{
enum dpll_pin_state state;
struct dpll_pin_ref *ref;
struct dpll_pin *ppin;
struct nlattr *nest;
unsigned long index;
int ret;
xa_for_each(&pin->parent_refs, index, ref) {
const struct dpll_pin_ops *ops = dpll_pin_ops(ref);
void *parent_priv;
ppin = ref->pin;
parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, ppin);
ret = ops->state_on_pin_get(pin,
dpll_pin_on_pin_priv(ppin, pin),
ppin, parent_priv, &state, extack);
if (ret)
return ret;
nest = nla_nest_start(msg, DPLL_A_PIN_PARENT_PIN);
if (!nest)
return -EMSGSIZE;
ret = dpll_msg_add_dev_parent_handle(msg, ppin->id);
if (ret)
goto nest_cancel;
if (nla_put_u32(msg, DPLL_A_PIN_STATE, state)) {
ret = -EMSGSIZE;
goto nest_cancel;
}
nla_nest_end(msg, nest);
}
return 0;
nest_cancel:
nla_nest_cancel(msg, nest);
return ret;
}
static int
dpll_msg_add_pin_dplls(struct sk_buff *msg, struct dpll_pin *pin,
struct netlink_ext_ack *extack)
{
struct dpll_pin_ref *ref;
struct nlattr *attr;
unsigned long index;
int ret;
xa_for_each(&pin->dpll_refs, index, ref) {
attr = nla_nest_start(msg, DPLL_A_PIN_PARENT_DEVICE);
if (!attr)
return -EMSGSIZE;
ret = dpll_msg_add_dev_parent_handle(msg, ref->dpll->id);
if (ret)
goto nest_cancel;
ret = dpll_msg_add_pin_on_dpll_state(msg, pin, ref, extack);
if (ret)
goto nest_cancel;
ret = dpll_msg_add_pin_operstate(msg, pin, ref, extack);
if (ret)
goto nest_cancel;
ret = dpll_msg_add_pin_prio(msg, pin, ref, extack);
if (ret)
goto nest_cancel;
ret = dpll_msg_add_pin_direction(msg, pin, ref, extack);
if (ret)
goto nest_cancel;
ret = dpll_msg_add_phase_offset(msg, pin, ref, extack);
if (ret)
goto nest_cancel;
ret = dpll_msg_add_ffo(msg, pin, ref,
DPLL_FFO_PIN_DEVICE, extack);
if (ret)
goto nest_cancel;
nla_nest_end(msg, attr);
}
return 0;
nest_cancel:
nla_nest_end(msg, attr);
return ret;
}
static int
dpll_cmd_pin_get_one(struct sk_buff *msg, struct dpll_pin *pin,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_properties *prop = &pin->prop;
struct dpll_pin_ref *ref;
int ret;
ref = dpll_pin_own_dpll_ref_first(pin);
if (!ref)
ref = dpll_xa_ref_dpll_first(&pin->dpll_refs);
ASSERT_NOT_NULL(ref);
ret = dpll_msg_add_pin_handle(msg, pin);
if (ret)
return ret;
if (nla_put_string(msg, DPLL_A_PIN_MODULE_NAME,
pin->module_name))
return -EMSGSIZE;
if (nla_put_64bit(msg, DPLL_A_PIN_CLOCK_ID, sizeof(pin->clock_id),
&pin->clock_id, DPLL_A_PIN_PAD))
return -EMSGSIZE;
if (prop->board_label &&
nla_put_string(msg, DPLL_A_PIN_BOARD_LABEL, prop->board_label))
return -EMSGSIZE;
if (prop->panel_label &&
nla_put_string(msg, DPLL_A_PIN_PANEL_LABEL, prop->panel_label))
return -EMSGSIZE;
if (prop->package_label &&
nla_put_string(msg, DPLL_A_PIN_PACKAGE_LABEL,
prop->package_label))
return -EMSGSIZE;
if (nla_put_u32(msg, DPLL_A_PIN_TYPE, prop->type))
return -EMSGSIZE;
if (nla_put_u32(msg, DPLL_A_PIN_CAPABILITIES, prop->capabilities))
return -EMSGSIZE;
ret = dpll_msg_add_pin_freq(msg, pin, ref, extack);
if (ret)
return ret;
if (prop->phase_gran &&
nla_put_u32(msg, DPLL_A_PIN_PHASE_ADJUST_GRAN,
prop->phase_gran))
return -EMSGSIZE;
if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MIN,
prop->phase_range.min))
return -EMSGSIZE;
if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MAX,
prop->phase_range.max))
return -EMSGSIZE;
ret = dpll_msg_add_pin_phase_adjust(msg, pin, ref, extack);
if (ret)
return ret;
ret = dpll_msg_add_ffo(msg, pin, ref,
DPLL_FFO_PORT_RXTX_RATE, extack);
if (ret)
return ret;
ret = dpll_msg_add_measured_freq(msg, pin, ref, extack);
if (ret)
return ret;
ret = dpll_msg_add_pin_esync(msg, pin, ref, extack);
if (ret)
return ret;
if (!xa_empty(&pin->ref_sync_pins))
ret = dpll_msg_add_pin_ref_sync(msg, pin, ref, extack);
if (ret)
return ret;
if (xa_empty(&pin->parent_refs))
ret = dpll_msg_add_pin_dplls(msg, pin, extack);
else
ret = dpll_msg_add_pin_parents(msg, pin, ref, extack);
return ret;
}
static int
dpll_device_get_one(struct dpll_device *dpll, struct sk_buff *msg,
struct netlink_ext_ack *extack)
{
int ret;
ret = dpll_msg_add_dev_handle(msg, dpll);
if (ret)
return ret;
if (nla_put_string(msg, DPLL_A_MODULE_NAME, module_name(dpll->module)))
return -EMSGSIZE;
if (nla_put_64bit(msg, DPLL_A_CLOCK_ID, sizeof(dpll->clock_id),
&dpll->clock_id, DPLL_A_PAD))
return -EMSGSIZE;
ret = dpll_msg_add_temp(msg, dpll, extack);
if (ret)
return ret;
ret = dpll_msg_add_lock_status(msg, dpll, extack);
if (ret)
return ret;
ret = dpll_msg_add_clock_quality_level(msg, dpll, extack);
if (ret)
return ret;
ret = dpll_msg_add_mode(msg, dpll, extack);
if (ret)
return ret;
ret = dpll_msg_add_mode_supported(msg, dpll, extack);
if (ret)
return ret;
if (nla_put_u32(msg, DPLL_A_TYPE, dpll->type))
return -EMSGSIZE;
ret = dpll_msg_add_phase_offset_monitor(msg, dpll, extack);
if (ret)
return ret;
ret = dpll_msg_add_phase_offset_avg_factor(msg, dpll, extack);
if (ret)
return ret;
ret = dpll_msg_add_freq_monitor(msg, dpll, extack);
if (ret)
return ret;
return 0;
}
static int
dpll_device_event_send(enum dpll_cmd event, struct dpll_device *dpll)
{
struct sk_buff *msg;
int ret = -ENOMEM;
void *hdr;
if (WARN_ON(!xa_get_mark(&dpll_device_xa, dpll->id, DPLL_REGISTERED)))
return -ENODEV;
msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event);
if (!hdr)
goto err_free_msg;
ret = dpll_device_get_one(dpll, msg, NULL);
if (ret)
goto err_cancel_msg;
genlmsg_end(msg, hdr);
genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL);
return 0;
err_cancel_msg:
genlmsg_cancel(msg, hdr);
err_free_msg:
nlmsg_free(msg);
return ret;
}
int dpll_device_create_ntf(struct dpll_device *dpll)
{
dpll_device_notify(dpll, DPLL_DEVICE_CREATED);
return dpll_device_event_send(DPLL_CMD_DEVICE_CREATE_NTF, dpll);
}
int dpll_device_delete_ntf(struct dpll_device *dpll)
{
dpll_device_notify(dpll, DPLL_DEVICE_DELETED);
return dpll_device_event_send(DPLL_CMD_DEVICE_DELETE_NTF, dpll);
}
int __dpll_device_change_ntf(struct dpll_device *dpll)
{
lockdep_assert_held(&dpll_lock);
dpll_device_notify(dpll, DPLL_DEVICE_CHANGED);
return dpll_device_event_send(DPLL_CMD_DEVICE_CHANGE_NTF, dpll);
}
EXPORT_SYMBOL_GPL(__dpll_device_change_ntf);
int dpll_device_change_ntf(struct dpll_device *dpll)
{
int ret;
mutex_lock(&dpll_lock);
ret = __dpll_device_change_ntf(dpll);
mutex_unlock(&dpll_lock);
return ret;
}
EXPORT_SYMBOL_GPL(dpll_device_change_ntf);
static int
dpll_pin_event_send(enum dpll_cmd event, struct dpll_pin *pin)
{
struct sk_buff *msg;
int ret = -ENOMEM;
void *hdr;
if (!dpll_pin_available(pin))
return -ENODEV;
msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event);
if (!hdr)
goto err_free_msg;
ret = dpll_cmd_pin_get_one(msg, pin, NULL);
if (ret)
goto err_cancel_msg;
genlmsg_end(msg, hdr);
genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL);
return 0;
err_cancel_msg:
genlmsg_cancel(msg, hdr);
err_free_msg:
nlmsg_free(msg);
return ret;
}
int dpll_pin_create_ntf(struct dpll_pin *pin, u64 src_clock_id)
{
dpll_pin_notify(pin, src_clock_id, DPLL_PIN_CREATED);
return dpll_pin_event_send(DPLL_CMD_PIN_CREATE_NTF, pin);
}
int dpll_pin_delete_ntf(struct dpll_pin *pin, u64 src_clock_id)
{
dpll_pin_notify(pin, src_clock_id, DPLL_PIN_DELETED);
return dpll_pin_event_send(DPLL_CMD_PIN_DELETE_NTF, pin);
}
int __dpll_pin_change_ntf(struct dpll_pin *pin)
{
lockdep_assert_held(&dpll_lock);
dpll_pin_notify(pin, pin->clock_id, DPLL_PIN_CHANGED);
return dpll_pin_event_send(DPLL_CMD_PIN_CHANGE_NTF, pin);
}
EXPORT_SYMBOL_GPL(__dpll_pin_change_ntf);
int dpll_pin_change_ntf(struct dpll_pin *pin)
{
int ret;
mutex_lock(&dpll_lock);
ret = __dpll_pin_change_ntf(pin);
mutex_unlock(&dpll_lock);
return ret;
}
EXPORT_SYMBOL_GPL(dpll_pin_change_ntf);
static int
dpll_mode_set(struct dpll_device *dpll, struct nlattr *a,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 };
enum dpll_mode mode = nla_get_u32(a), old_mode;
int ret;
if (!(ops->mode_set && ops->supported_modes_get)) {
NL_SET_ERR_MSG_ATTR(extack, a,
"dpll device does not support mode switch");
return -EOPNOTSUPP;
}
ret = ops->mode_get(dpll, dpll_priv(dpll), &old_mode, extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get current mode");
return ret;
}
if (mode == old_mode)
return 0;
ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes, extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get supported modes");
return ret;
}
if (!test_bit(mode, modes)) {
NL_SET_ERR_MSG(extack,
"dpll device does not support requested mode");
return -EINVAL;
}
return ops->mode_set(dpll, dpll_priv(dpll), mode, extack);
}
static int
dpll_phase_offset_monitor_set(struct dpll_device *dpll, struct nlattr *a,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_feature_state state = nla_get_u32(a), old_state;
int ret;
if (!(ops->phase_offset_monitor_set && ops->phase_offset_monitor_get)) {
NL_SET_ERR_MSG_ATTR(extack, a, "dpll device not capable of phase offset monitor");
return -EOPNOTSUPP;
}
ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll), &old_state,
extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get current state of phase offset monitor");
return ret;
}
if (state == old_state)
return 0;
return ops->phase_offset_monitor_set(dpll, dpll_priv(dpll), state,
extack);
}
static int
dpll_phase_offset_avg_factor_set(struct dpll_device *dpll, struct nlattr *a,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
u32 factor = nla_get_u32(a);
if (!ops->phase_offset_avg_factor_set) {
NL_SET_ERR_MSG_ATTR(extack, a,
"device not capable of changing phase offset average factor");
return -EOPNOTSUPP;
}
return ops->phase_offset_avg_factor_set(dpll, dpll_priv(dpll), factor,
extack);
}
static int
dpll_freq_monitor_set(struct dpll_device *dpll, struct nlattr *a,
struct netlink_ext_ack *extack)
{
const struct dpll_device_ops *ops = dpll_device_ops(dpll);
enum dpll_feature_state state = nla_get_u32(a), old_state;
int ret;
if (!(ops->freq_monitor_set && ops->freq_monitor_get)) {
NL_SET_ERR_MSG_ATTR(extack, a,
"dpll device not capable of frequency monitor");
return -EOPNOTSUPP;
}
ret = ops->freq_monitor_get(dpll, dpll_priv(dpll), &old_state,
extack);
if (ret) {
NL_SET_ERR_MSG(extack,
"unable to get current state of frequency monitor");
return ret;
}
if (state == old_state)
return 0;
return ops->freq_monitor_set(dpll, dpll_priv(dpll), state, extack);
}
static int
dpll_pin_freq_set(struct dpll_pin *pin, struct nlattr *a,
struct netlink_ext_ack *extack)
{
u64 freq = nla_get_u64(a), old_freq;
struct dpll_pin_ref *ref, *failed;
const struct dpll_pin_ops *ops;
struct dpll_device *dpll;
unsigned long i;
int ret;
if (!dpll_pin_is_freq_supported(pin, freq)) {
NL_SET_ERR_MSG_ATTR(extack, a, "frequency is not supported by the device");
return -EINVAL;
}
xa_for_each(&pin->dpll_refs, i, ref) {
ops = dpll_pin_ops(ref);
if ((!ops->frequency_set || !ops->frequency_get) &&
ref->dpll->module == pin->module &&
ref->dpll->clock_id == pin->clock_id) {
NL_SET_ERR_MSG(extack,
"frequency set not supported by the device");
return -EOPNOTSUPP;
}
}
ref = dpll_pin_own_dpll_ref_first(pin);
if (!ref) {
NL_SET_ERR_MSG(extack, "pin owner dpll not found");
return -ENODEV;
}
ops = dpll_pin_ops(ref);
dpll = ref->dpll;
ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), &old_freq, extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get old frequency value");
return ret;
}
if (freq == old_freq)
return 0;
xa_for_each(&pin->dpll_refs, i, ref) {
ops = dpll_pin_ops(ref);
if (!ops->frequency_set)
continue;
dpll = ref->dpll;
ret = ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), freq, extack);
if (ret) {
failed = ref;
NL_SET_ERR_MSG_FMT(extack, "frequency set failed for dpll_id:%u",
dpll->id);
goto rollback;
}
}
__dpll_pin_change_ntf(pin);
return 0;
rollback:
xa_for_each(&pin->dpll_refs, i, ref) {
if (ref == failed)
break;
ops = dpll_pin_ops(ref);
if (!ops->frequency_set)
continue;
dpll = ref->dpll;
if (ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), old_freq, extack))
NL_SET_ERR_MSG(extack, "set frequency rollback failed");
}
return ret;
}
static int
dpll_pin_esync_set(struct dpll_pin *pin, struct nlattr *a,
struct netlink_ext_ack *extack)
{
struct dpll_pin_ref *ref, *failed;
const struct dpll_pin_ops *ops;
struct dpll_pin_esync esync;
u64 freq = nla_get_u64(a);
struct dpll_device *dpll;
bool supported = false;
unsigned long i;
int ret;
xa_for_each(&pin->dpll_refs, i, ref) {
ops = dpll_pin_ops(ref);
if ((!ops->esync_set || !ops->esync_get) &&
ref->dpll->module == pin->module &&
ref->dpll->clock_id == pin->clock_id) {
NL_SET_ERR_MSG(extack,
"embedded sync feature is not supported by this device");
return -EOPNOTSUPP;
}
}
ref = dpll_pin_own_dpll_ref_first(pin);
if (!ref) {
NL_SET_ERR_MSG(extack, "pin owner dpll not found");
return -ENODEV;
}
ops = dpll_pin_ops(ref);
dpll = ref->dpll;
ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), &esync, extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get current embedded sync frequency value");
return ret;
}
if (freq == esync.freq)
return 0;
for (i = 0; i < esync.range_num; i++)
if (freq <= esync.range[i].max && freq >= esync.range[i].min)
supported = true;
if (!supported) {
NL_SET_ERR_MSG_ATTR(extack, a,
"requested embedded sync frequency value is not supported by this device");
return -EINVAL;
}
xa_for_each(&pin->dpll_refs, i, ref) {
void *pin_dpll_priv;
ops = dpll_pin_ops(ref);
if (!ops->esync_set)
continue;
dpll = ref->dpll;
pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin);
ret = ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll),
freq, extack);
if (ret) {
failed = ref;
NL_SET_ERR_MSG_FMT(extack,
"embedded sync frequency set failed for dpll_id: %u",
dpll->id);
goto rollback;
}
}
__dpll_pin_change_ntf(pin);
return 0;
rollback:
xa_for_each(&pin->dpll_refs, i, ref) {
void *pin_dpll_priv;
if (ref == failed)
break;
ops = dpll_pin_ops(ref);
if (!ops->esync_set)
continue;
dpll = ref->dpll;
pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin);
if (ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll),
esync.freq, extack))
NL_SET_ERR_MSG(extack, "set embedded sync frequency rollback failed");
}
return ret;
}
static int
dpll_pin_ref_sync_state_set(struct dpll_pin *pin,
unsigned long ref_sync_pin_idx,
const enum dpll_pin_state state,
struct netlink_ext_ack *extack)
{
struct dpll_pin_ref *ref, *failed;
const struct dpll_pin_ops *ops;
enum dpll_pin_state old_state;
struct dpll_pin *ref_sync_pin;
struct dpll_device *dpll;
unsigned long i;
int ret;
ref_sync_pin = xa_find(&pin->ref_sync_pins, &ref_sync_pin_idx,
ULONG_MAX, XA_PRESENT);
if (!ref_sync_pin) {
NL_SET_ERR_MSG(extack, "reference sync pin not found");
return -EINVAL;
}
if (!dpll_pin_available(ref_sync_pin)) {
NL_SET_ERR_MSG(extack, "reference sync pin not available");
return -EINVAL;
}
ref = dpll_pin_own_dpll_ref_first(pin);
if (!ref) {
NL_SET_ERR_MSG(extack, "pin owner dpll not found");
return -ENODEV;
}
ops = dpll_pin_ops(ref);
if (!ops->ref_sync_set || !ops->ref_sync_get) {
NL_SET_ERR_MSG(extack, "reference sync not supported by this pin");
return -EOPNOTSUPP;
}
dpll = ref->dpll;
ret = ops->ref_sync_get(pin, dpll_pin_on_dpll_priv(dpll, pin),
ref_sync_pin,
dpll_pin_on_dpll_priv(dpll, ref_sync_pin),
&old_state, extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get old reference sync state");
return ret;
}
if (state == old_state)
return 0;
xa_for_each(&pin->dpll_refs, i, ref) {
ops = dpll_pin_ops(ref);
if (!ops->ref_sync_set)
continue;
dpll = ref->dpll;
ret = ops->ref_sync_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
ref_sync_pin,
dpll_pin_on_dpll_priv(dpll,
ref_sync_pin),
state, extack);
if (ret) {
failed = ref;
NL_SET_ERR_MSG_FMT(extack, "reference sync set failed for dpll_id:%u",
dpll->id);
goto rollback;
}
}
__dpll_pin_change_ntf(pin);
return 0;
rollback:
xa_for_each(&pin->dpll_refs, i, ref) {
if (ref == failed)
break;
ops = dpll_pin_ops(ref);
if (!ops->ref_sync_set)
continue;
dpll = ref->dpll;
if (ops->ref_sync_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
ref_sync_pin,
dpll_pin_on_dpll_priv(dpll, ref_sync_pin),
old_state, extack))
NL_SET_ERR_MSG(extack, "set reference sync rollback failed");
}
return ret;
}
static int
dpll_pin_ref_sync_set(struct dpll_pin *pin, struct nlattr *nest,
struct netlink_ext_ack *extack)
{
struct nlattr *tb[DPLL_A_PIN_MAX + 1];
enum dpll_pin_state state;
u32 sync_pin_id;
nla_parse_nested(tb, DPLL_A_PIN_MAX, nest,
dpll_reference_sync_nl_policy, extack);
if (!tb[DPLL_A_PIN_ID]) {
NL_SET_ERR_MSG(extack, "sync pin id expected");
return -EINVAL;
}
sync_pin_id = nla_get_u32(tb[DPLL_A_PIN_ID]);
if (!tb[DPLL_A_PIN_STATE]) {
NL_SET_ERR_MSG(extack, "sync pin state expected");
return -EINVAL;
}
state = nla_get_u32(tb[DPLL_A_PIN_STATE]);
return dpll_pin_ref_sync_state_set(pin, sync_pin_id, state, extack);
}
static int
dpll_pin_on_pin_state_set(struct dpll_pin *pin, u32 parent_idx,
enum dpll_pin_state state,
struct netlink_ext_ack *extack)
{
struct dpll_pin_ref *parent_ref;
const struct dpll_pin_ops *ops;
struct dpll_pin_ref *dpll_ref;
void *pin_priv, *parent_priv;
struct dpll_pin *parent;
unsigned long i;
int ret;
if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE &
pin->prop.capabilities) && !pin->fwnode) {
NL_SET_ERR_MSG(extack, "state changing is not allowed");
return -EOPNOTSUPP;
}
parent = xa_load(&dpll_pin_xa, parent_idx);
if (!parent)
return -EINVAL;
parent_ref = xa_load(&pin->parent_refs, parent->pin_idx);
if (!parent_ref)
return -EINVAL;
xa_for_each(&parent->dpll_refs, i, dpll_ref) {
ops = dpll_pin_ops(parent_ref);
if (!ops->state_on_pin_set)
return -EOPNOTSUPP;
pin_priv = dpll_pin_on_pin_priv(parent, pin);
parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, parent);
ret = ops->state_on_pin_set(pin, pin_priv, parent, parent_priv,
state, extack);
if (ret)
return ret;
}
__dpll_pin_change_ntf(pin);
return 0;
}
static int
dpll_pin_state_set(struct dpll_device *dpll, struct dpll_pin *pin,
enum dpll_pin_state state,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops;
struct dpll_pin_ref *ref;
int ret;
if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE &
pin->prop.capabilities) && !pin->fwnode) {
NL_SET_ERR_MSG(extack, "state changing is not allowed");
return -EOPNOTSUPP;
}
ref = xa_load(&pin->dpll_refs, dpll->id);
ASSERT_NOT_NULL(ref);
ops = dpll_pin_ops(ref);
if (!ops->state_on_dpll_set)
return -EOPNOTSUPP;
ret = ops->state_on_dpll_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), state, extack);
if (ret)
return ret;
__dpll_pin_change_ntf(pin);
return 0;
}
static int
dpll_pin_prio_set(struct dpll_device *dpll, struct dpll_pin *pin,
u32 prio, struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops;
struct dpll_pin_ref *ref;
int ret;
if (!(DPLL_PIN_CAPABILITIES_PRIORITY_CAN_CHANGE &
pin->prop.capabilities)) {
NL_SET_ERR_MSG(extack, "prio changing is not allowed");
return -EOPNOTSUPP;
}
ref = xa_load(&pin->dpll_refs, dpll->id);
ASSERT_NOT_NULL(ref);
ops = dpll_pin_ops(ref);
if (!ops->prio_set)
return -EOPNOTSUPP;
ret = ops->prio_set(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll,
dpll_priv(dpll), prio, extack);
if (ret)
return ret;
__dpll_pin_change_ntf(pin);
return 0;
}
static int
dpll_pin_direction_set(struct dpll_pin *pin, struct dpll_device *dpll,
enum dpll_pin_direction direction,
struct netlink_ext_ack *extack)
{
const struct dpll_pin_ops *ops;
struct dpll_pin_ref *ref;
int ret;
if (!(DPLL_PIN_CAPABILITIES_DIRECTION_CAN_CHANGE &
pin->prop.capabilities)) {
NL_SET_ERR_MSG(extack, "direction changing is not allowed");
return -EOPNOTSUPP;
}
ref = xa_load(&pin->dpll_refs, dpll->id);
ASSERT_NOT_NULL(ref);
ops = dpll_pin_ops(ref);
if (!ops->direction_set)
return -EOPNOTSUPP;
ret = ops->direction_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), direction, extack);
if (ret)
return ret;
__dpll_pin_change_ntf(pin);
return 0;
}
static int
dpll_pin_phase_adj_set(struct dpll_pin *pin, struct nlattr *phase_adj_attr,
struct netlink_ext_ack *extack)
{
struct dpll_pin_ref *ref, *failed;
const struct dpll_pin_ops *ops;
s32 phase_adj, old_phase_adj;
struct dpll_device *dpll;
unsigned long i;
int ret;
phase_adj = nla_get_s32(phase_adj_attr);
if (phase_adj > pin->prop.phase_range.max ||
phase_adj < pin->prop.phase_range.min) {
NL_SET_ERR_MSG_ATTR(extack, phase_adj_attr,
"phase adjust value of out range");
return -EINVAL;
}
if (pin->prop.phase_gran && phase_adj % (s32)pin->prop.phase_gran) {
NL_SET_ERR_MSG_ATTR_FMT(extack, phase_adj_attr,
"phase adjust value not multiple of %u",
pin->prop.phase_gran);
return -EINVAL;
}
xa_for_each(&pin->dpll_refs, i, ref) {
ops = dpll_pin_ops(ref);
if ((!ops->phase_adjust_set || !ops->phase_adjust_get) &&
ref->dpll->module == pin->module &&
ref->dpll->clock_id == pin->clock_id) {
NL_SET_ERR_MSG(extack, "phase adjust not supported");
return -EOPNOTSUPP;
}
}
ref = dpll_pin_own_dpll_ref_first(pin);
if (!ref) {
NL_SET_ERR_MSG(extack, "pin owner dpll not found");
return -ENODEV;
}
ops = dpll_pin_ops(ref);
dpll = ref->dpll;
ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), &old_phase_adj,
extack);
if (ret) {
NL_SET_ERR_MSG(extack, "unable to get old phase adjust value");
return ret;
}
if (phase_adj == old_phase_adj)
return 0;
xa_for_each(&pin->dpll_refs, i, ref) {
ops = dpll_pin_ops(ref);
if (!ops->phase_adjust_set)
continue;
dpll = ref->dpll;
ret = ops->phase_adjust_set(pin,
dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), phase_adj,
extack);
if (ret) {
failed = ref;
NL_SET_ERR_MSG_FMT(extack,
"phase adjust set failed for dpll_id:%u",
dpll->id);
goto rollback;
}
}
__dpll_pin_change_ntf(pin);
return 0;
rollback:
xa_for_each(&pin->dpll_refs, i, ref) {
if (ref == failed)
break;
ops = dpll_pin_ops(ref);
if (!ops->phase_adjust_set)
continue;
dpll = ref->dpll;
if (ops->phase_adjust_set(pin, dpll_pin_on_dpll_priv(dpll, pin),
dpll, dpll_priv(dpll), old_phase_adj,
extack))
NL_SET_ERR_MSG(extack, "set phase adjust rollback failed");
}
return ret;
}
static int
dpll_pin_parent_device_set(struct dpll_pin *pin, struct nlattr *parent_nest,
struct netlink_ext_ack *extack)
{
struct nlattr *tb[DPLL_A_PIN_MAX + 1];
enum dpll_pin_direction direction;
enum dpll_pin_state state;
struct dpll_pin_ref *ref;
struct dpll_device *dpll;
u32 pdpll_idx, prio;
int ret;
nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest,
dpll_pin_parent_device_nl_policy, extack);
if (!tb[DPLL_A_PIN_PARENT_ID]) {
NL_SET_ERR_MSG(extack, "device parent id expected");
return -EINVAL;
}
pdpll_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]);
dpll = xa_load(&dpll_device_xa, pdpll_idx);
if (!dpll) {
NL_SET_ERR_MSG(extack, "parent device not found");
return -EINVAL;
}
ref = xa_load(&pin->dpll_refs, dpll->id);
if (!ref) {
NL_SET_ERR_MSG(extack, "pin not connected to given parent device");
return -EINVAL;
}
if (tb[DPLL_A_PIN_STATE]) {
state = nla_get_u32(tb[DPLL_A_PIN_STATE]);
ret = dpll_pin_state_set(dpll, pin, state, extack);
if (ret)
return ret;
}
if (tb[DPLL_A_PIN_PRIO]) {
prio = nla_get_u32(tb[DPLL_A_PIN_PRIO]);
ret = dpll_pin_prio_set(dpll, pin, prio, extack);
if (ret)
return ret;
}
if (tb[DPLL_A_PIN_DIRECTION]) {
direction = nla_get_u32(tb[DPLL_A_PIN_DIRECTION]);
ret = dpll_pin_direction_set(pin, dpll, direction, extack);
if (ret)
return ret;
}
return 0;
}
static int
dpll_pin_parent_pin_set(struct dpll_pin *pin, struct nlattr *parent_nest,
struct netlink_ext_ack *extack)
{
struct nlattr *tb[DPLL_A_PIN_MAX + 1];
u32 ppin_idx;
int ret;
nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest,
dpll_pin_parent_pin_nl_policy, extack);
if (!tb[DPLL_A_PIN_PARENT_ID]) {
NL_SET_ERR_MSG(extack, "device parent id expected");
return -EINVAL;
}
ppin_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]);
if (tb[DPLL_A_PIN_STATE]) {
enum dpll_pin_state state = nla_get_u32(tb[DPLL_A_PIN_STATE]);
ret = dpll_pin_on_pin_state_set(pin, ppin_idx, state, extack);
if (ret)
return ret;
}
return 0;
}
static int
dpll_pin_set_from_nlattr(struct dpll_pin *pin, struct genl_info *info)
{
struct nlattr *a;
int rem, ret;
nla_for_each_attr(a, genlmsg_data(info->genlhdr),
genlmsg_len(info->genlhdr), rem) {
switch (nla_type(a)) {
case DPLL_A_PIN_FREQUENCY:
ret = dpll_pin_freq_set(pin, a, info->extack);
if (ret)
return ret;
break;
case DPLL_A_PIN_PHASE_ADJUST:
ret = dpll_pin_phase_adj_set(pin, a, info->extack);
if (ret)
return ret;
break;
case DPLL_A_PIN_PARENT_DEVICE:
ret = dpll_pin_parent_device_set(pin, a, info->extack);
if (ret)
return ret;
break;
case DPLL_A_PIN_PARENT_PIN:
ret = dpll_pin_parent_pin_set(pin, a, info->extack);
if (ret)
return ret;
break;
case DPLL_A_PIN_ESYNC_FREQUENCY:
ret = dpll_pin_esync_set(pin, a, info->extack);
if (ret)
return ret;
break;
case DPLL_A_PIN_REFERENCE_SYNC:
ret = dpll_pin_ref_sync_set(pin, a, info->extack);
if (ret)
return ret;
break;
}
}
return 0;
}
static struct dpll_pin *
dpll_pin_find(u64 clock_id, struct nlattr *mod_name_attr,
enum dpll_pin_type type, struct nlattr *board_label,
struct nlattr *panel_label, struct nlattr *package_label,
struct netlink_ext_ack *extack)
{
bool board_match, panel_match, package_match;
struct dpll_pin *pin_match = NULL, *pin;
const struct dpll_pin_properties *prop;
bool cid_match, mod_match, type_match;
unsigned long i;
xa_for_each_marked(&dpll_pin_xa, i, pin, DPLL_REGISTERED) {
prop = &pin->prop;
cid_match = clock_id ? pin->clock_id == clock_id : true;
mod_match = mod_name_attr && pin->module_name[0] ?
!nla_strcmp(mod_name_attr,
pin->module_name) : true;
type_match = type ? prop->type == type : true;
board_match = board_label ? (prop->board_label ?
!nla_strcmp(board_label, prop->board_label) : false) :
true;
panel_match = panel_label ? (prop->panel_label ?
!nla_strcmp(panel_label, prop->panel_label) : false) :
true;
package_match = package_label ? (prop->package_label ?
!nla_strcmp(package_label, prop->package_label) :
false) : true;
if (cid_match && mod_match && type_match && board_match &&
panel_match && package_match) {
if (pin_match) {
NL_SET_ERR_MSG(extack, "multiple matches");
return ERR_PTR(-EINVAL);
}
pin_match = pin;
}
}
if (!pin_match) {
NL_SET_ERR_MSG(extack, "not found");
return ERR_PTR(-ENODEV);
}
return pin_match;
}
static struct dpll_pin *dpll_pin_find_from_nlattr(struct genl_info *info)
{
struct nlattr *attr, *mod_name_attr = NULL, *board_label_attr = NULL,
*panel_label_attr = NULL, *package_label_attr = NULL;
enum dpll_pin_type type = 0;
u64 clock_id = 0;
int rem = 0;
nla_for_each_attr(attr, genlmsg_data(info->genlhdr),
genlmsg_len(info->genlhdr), rem) {
switch (nla_type(attr)) {
case DPLL_A_PIN_CLOCK_ID:
if (clock_id)
goto duplicated_attr;
clock_id = nla_get_u64(attr);
break;
case DPLL_A_PIN_MODULE_NAME:
if (mod_name_attr)
goto duplicated_attr;
mod_name_attr = attr;
break;
case DPLL_A_PIN_TYPE:
if (type)
goto duplicated_attr;
type = nla_get_u32(attr);
break;
case DPLL_A_PIN_BOARD_LABEL:
if (board_label_attr)
goto duplicated_attr;
board_label_attr = attr;
break;
case DPLL_A_PIN_PANEL_LABEL:
if (panel_label_attr)
goto duplicated_attr;
panel_label_attr = attr;
break;
case DPLL_A_PIN_PACKAGE_LABEL:
if (package_label_attr)
goto duplicated_attr;
package_label_attr = attr;
break;
default:
break;
}
}
if (!(clock_id || mod_name_attr || board_label_attr ||
panel_label_attr || package_label_attr)) {
NL_SET_ERR_MSG(info->extack, "missing attributes");
return ERR_PTR(-EINVAL);
}
return dpll_pin_find(clock_id, mod_name_attr, type, board_label_attr,
panel_label_attr, package_label_attr,
info->extack);
duplicated_attr:
NL_SET_ERR_MSG(info->extack, "duplicated attribute");
return ERR_PTR(-EINVAL);
}
int dpll_nl_pin_id_get_doit(struct sk_buff *skb, struct genl_info *info)
{
struct dpll_pin *pin;
struct sk_buff *msg;
struct nlattr *hdr;
int ret;
msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!msg)
return -ENOMEM;
hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0,
DPLL_CMD_PIN_ID_GET);
if (!hdr) {
nlmsg_free(msg);
return -EMSGSIZE;
}
pin = dpll_pin_find_from_nlattr(info);
if (IS_ERR(pin)) {
nlmsg_free(msg);
return PTR_ERR(pin);
}
if (!dpll_pin_available(pin)) {
nlmsg_free(msg);
return -ENODEV;
}
ret = dpll_msg_add_pin_handle(msg, pin);
if (ret) {
nlmsg_free(msg);
return ret;
}
genlmsg_end(msg, hdr);
return genlmsg_reply(msg, info);
}
int dpll_nl_pin_get_doit(struct sk_buff *skb, struct genl_info *info)
{
struct dpll_pin *pin = info->user_ptr[0];
struct sk_buff *msg;
struct nlattr *hdr;
int ret;
if (!pin)
return -ENODEV;
msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!msg)
return -ENOMEM;
hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0,
DPLL_CMD_PIN_GET);
if (!hdr) {
nlmsg_free(msg);
return -EMSGSIZE;
}
ret = dpll_cmd_pin_get_one(msg, pin, info->extack);
if (ret) {
nlmsg_free(msg);
return ret;
}
genlmsg_end(msg, hdr);
return genlmsg_reply(msg, info);
}
int dpll_nl_pin_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
{
struct dpll_dump_ctx *ctx = dpll_dump_context(cb);
struct dpll_pin *pin;
struct nlattr *hdr;
unsigned long i;
int ret = 0;
mutex_lock(&dpll_lock);
xa_for_each_marked_start(&dpll_pin_xa, i, pin, DPLL_REGISTERED,
ctx->idx) {
if (!dpll_pin_available(pin))
continue;
hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
cb->nlh->nlmsg_seq,
&dpll_nl_family, NLM_F_MULTI,
DPLL_CMD_PIN_GET);
if (!hdr) {
ret = -EMSGSIZE;
break;
}
ret = dpll_cmd_pin_get_one(skb, pin, cb->extack);
if (ret) {
genlmsg_cancel(skb, hdr);
break;
}
genlmsg_end(skb, hdr);
}
mutex_unlock(&dpll_lock);
if (ret == -EMSGSIZE) {
ctx->idx = i;
return skb->len;
}
return ret;
}
int dpll_nl_pin_set_doit(struct sk_buff *skb, struct genl_info *info)
{
struct dpll_pin *pin = info->user_ptr[0];
return dpll_pin_set_from_nlattr(pin, info);
}
static struct dpll_device *
dpll_device_find(u64 clock_id, struct nlattr *mod_name_attr,
enum dpll_type type, struct netlink_ext_ack *extack)
{
struct dpll_device *dpll_match = NULL, *dpll;
bool cid_match, mod_match, type_match;
unsigned long i;
xa_for_each_marked(&dpll_device_xa, i, dpll, DPLL_REGISTERED) {
cid_match = clock_id ? dpll->clock_id == clock_id : true;
mod_match = mod_name_attr ? (module_name(dpll->module) ?
!nla_strcmp(mod_name_attr,
module_name(dpll->module)) : false) : true;
type_match = type ? dpll->type == type : true;
if (cid_match && mod_match && type_match) {
if (dpll_match) {
NL_SET_ERR_MSG(extack, "multiple matches");
return ERR_PTR(-EINVAL);
}
dpll_match = dpll;
}
}
if (!dpll_match) {
NL_SET_ERR_MSG(extack, "not found");
return ERR_PTR(-ENODEV);
}
return dpll_match;
}
static struct dpll_device *
dpll_device_find_from_nlattr(struct genl_info *info)
{
struct nlattr *attr, *mod_name_attr = NULL;
enum dpll_type type = 0;
u64 clock_id = 0;
int rem = 0;
nla_for_each_attr(attr, genlmsg_data(info->genlhdr),
genlmsg_len(info->genlhdr), rem) {
switch (nla_type(attr)) {
case DPLL_A_CLOCK_ID:
if (clock_id)
goto duplicated_attr;
clock_id = nla_get_u64(attr);
break;
case DPLL_A_MODULE_NAME:
if (mod_name_attr)
goto duplicated_attr;
mod_name_attr = attr;
break;
case DPLL_A_TYPE:
if (type)
goto duplicated_attr;
type = nla_get_u32(attr);
break;
default:
break;
}
}
if (!clock_id && !mod_name_attr && !type) {
NL_SET_ERR_MSG(info->extack, "missing attributes");
return ERR_PTR(-EINVAL);
}
return dpll_device_find(clock_id, mod_name_attr, type, info->extack);
duplicated_attr:
NL_SET_ERR_MSG(info->extack, "duplicated attribute");
return ERR_PTR(-EINVAL);
}
int dpll_nl_device_id_get_doit(struct sk_buff *skb, struct genl_info *info)
{
struct dpll_device *dpll;
struct sk_buff *msg;
struct nlattr *hdr;
int ret;
msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!msg)
return -ENOMEM;
hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0,
DPLL_CMD_DEVICE_ID_GET);
if (!hdr) {
nlmsg_free(msg);
return -EMSGSIZE;
}
dpll = dpll_device_find_from_nlattr(info);
if (IS_ERR(dpll)) {
nlmsg_free(msg);
return PTR_ERR(dpll);
}
ret = dpll_msg_add_dev_handle(msg, dpll);
if (ret) {
nlmsg_free(msg);
return ret;
}
genlmsg_end(msg, hdr);
return genlmsg_reply(msg, info);
}
int dpll_nl_device_get_doit(struct sk_buff *skb, struct genl_info *info)
{
struct dpll_device *dpll = info->user_ptr[0];
struct sk_buff *msg;
struct nlattr *hdr;
int ret;
msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!msg)
return -ENOMEM;
hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0,
DPLL_CMD_DEVICE_GET);
if (!hdr) {
nlmsg_free(msg);
return -EMSGSIZE;
}
ret = dpll_device_get_one(dpll, msg, info->extack);
if (ret) {
nlmsg_free(msg);
return ret;
}
genlmsg_end(msg, hdr);
return genlmsg_reply(msg, info);
}
static int
dpll_set_from_nlattr(struct dpll_device *dpll, struct genl_info *info)
{
struct nlattr *a;
int rem, ret;
nla_for_each_attr(a, genlmsg_data(info->genlhdr),
genlmsg_len(info->genlhdr), rem) {
switch (nla_type(a)) {
case DPLL_A_MODE:
ret = dpll_mode_set(dpll, a, info->extack);
if (ret)
return ret;
break;
case DPLL_A_PHASE_OFFSET_MONITOR:
ret = dpll_phase_offset_monitor_set(dpll, a,
info->extack);
if (ret)
return ret;
break;
case DPLL_A_PHASE_OFFSET_AVG_FACTOR:
ret = dpll_phase_offset_avg_factor_set(dpll, a,
info->extack);
if (ret)
return ret;
break;
case DPLL_A_FREQUENCY_MONITOR:
ret = dpll_freq_monitor_set(dpll, a,
info->extack);
if (ret)
return ret;
break;
}
}
return 0;
}
int dpll_nl_device_set_doit(struct sk_buff *skb, struct genl_info *info)
{
struct dpll_device *dpll = info->user_ptr[0];
return dpll_set_from_nlattr(dpll, info);
}
int dpll_nl_device_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
{
struct dpll_dump_ctx *ctx = dpll_dump_context(cb);
struct dpll_device *dpll;
struct nlattr *hdr;
unsigned long i;
int ret = 0;
mutex_lock(&dpll_lock);
xa_for_each_marked_start(&dpll_device_xa, i, dpll, DPLL_REGISTERED,
ctx->idx) {
hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
cb->nlh->nlmsg_seq, &dpll_nl_family,
NLM_F_MULTI, DPLL_CMD_DEVICE_GET);
if (!hdr) {
ret = -EMSGSIZE;
break;
}
ret = dpll_device_get_one(dpll, skb, cb->extack);
if (ret) {
genlmsg_cancel(skb, hdr);
break;
}
genlmsg_end(skb, hdr);
}
mutex_unlock(&dpll_lock);
if (ret == -EMSGSIZE) {
ctx->idx = i;
return skb->len;
}
return ret;
}
int dpll_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb,
struct genl_info *info)
{
u32 id;
if (GENL_REQ_ATTR_CHECK(info, DPLL_A_ID))
return -EINVAL;
mutex_lock(&dpll_lock);
id = nla_get_u32(info->attrs[DPLL_A_ID]);
info->user_ptr[0] = dpll_device_get_by_id(id);
if (!info->user_ptr[0]) {
NL_SET_ERR_MSG(info->extack, "device not found");
goto unlock;
}
return 0;
unlock:
mutex_unlock(&dpll_lock);
return -ENODEV;
}
void dpll_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb,
struct genl_info *info)
{
mutex_unlock(&dpll_lock);
}
int
dpll_lock_doit(const struct genl_split_ops *ops, struct sk_buff *skb,
struct genl_info *info)
{
mutex_lock(&dpll_lock);
return 0;
}
void
dpll_unlock_doit(const struct genl_split_ops *ops, struct sk_buff *skb,
struct genl_info *info)
{
mutex_unlock(&dpll_lock);
}
int dpll_pin_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb,
struct genl_info *info)
{
int ret;
mutex_lock(&dpll_lock);
if (GENL_REQ_ATTR_CHECK(info, DPLL_A_PIN_ID)) {
ret = -EINVAL;
goto unlock_dev;
}
info->user_ptr[0] = xa_load(&dpll_pin_xa,
nla_get_u32(info->attrs[DPLL_A_PIN_ID]));
if (!info->user_ptr[0] ||
!dpll_pin_available(info->user_ptr[0])) {
NL_SET_ERR_MSG(info->extack, "pin not found");
ret = -ENODEV;
goto unlock_dev;
}
return 0;
unlock_dev:
mutex_unlock(&dpll_lock);
return ret;
}
void dpll_pin_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb,
struct genl_info *info)
{
mutex_unlock(&dpll_lock);
}
