*/
#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/ptp_clock_kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
#include <linux/fsl/mc.h>
+#include <linux/fsl/ptp_qoriq.h>
#include "dpaa2-ptp.h"
-struct ptp_dpaa2_priv {
- struct fsl_mc_device *ptp_mc_dev;
- struct ptp_clock *clock;
- struct ptp_clock_info caps;
- u32 freq_comp;
-};
-
-/* PTP clock operations */
-static int ptp_dpaa2_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
-{
- struct ptp_dpaa2_priv *ptp_dpaa2 =
- container_of(ptp, struct ptp_dpaa2_priv, caps);
- struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
- struct device *dev = &mc_dev->dev;
- u64 adj;
- u32 diff, tmr_add;
- int neg_adj = 0;
- int err = 0;
-
- if (ppb < 0) {
- neg_adj = 1;
- ppb = -ppb;
- }
-
- tmr_add = ptp_dpaa2->freq_comp;
- adj = tmr_add;
- adj *= ppb;
- diff = div_u64(adj, 1000000000ULL);
-
- tmr_add = neg_adj ? tmr_add - diff : tmr_add + diff;
-
- err = dprtc_set_freq_compensation(mc_dev->mc_io, 0,
- mc_dev->mc_handle, tmr_add);
- if (err)
- dev_err(dev, "dprtc_set_freq_compensation err %d\n", err);
- return err;
-}
-
-static int ptp_dpaa2_adjtime(struct ptp_clock_info *ptp, s64 delta)
-{
- struct ptp_dpaa2_priv *ptp_dpaa2 =
- container_of(ptp, struct ptp_dpaa2_priv, caps);
- struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
- struct device *dev = &mc_dev->dev;
- s64 now;
- int err = 0;
-
- err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &now);
- if (err) {
- dev_err(dev, "dprtc_get_time err %d\n", err);
- return err;
- }
-
- now += delta;
-
- err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, now);
- if (err)
- dev_err(dev, "dprtc_set_time err %d\n", err);
- return err;
-}
-
-static int ptp_dpaa2_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
-{
- struct ptp_dpaa2_priv *ptp_dpaa2 =
- container_of(ptp, struct ptp_dpaa2_priv, caps);
- struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
- struct device *dev = &mc_dev->dev;
- u64 ns;
- u32 remainder;
- int err = 0;
-
- err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &ns);
- if (err) {
- dev_err(dev, "dprtc_get_time err %d\n", err);
- return err;
- }
-
- ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
- ts->tv_nsec = remainder;
- return err;
-}
-
-static int ptp_dpaa2_settime(struct ptp_clock_info *ptp,
- const struct timespec64 *ts)
-{
- struct ptp_dpaa2_priv *ptp_dpaa2 =
- container_of(ptp, struct ptp_dpaa2_priv, caps);
- struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
- struct device *dev = &mc_dev->dev;
- u64 ns;
- int err = 0;
-
- ns = ts->tv_sec * 1000000000ULL;
- ns += ts->tv_nsec;
-
- err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, ns);
- if (err)
- dev_err(dev, "dprtc_set_time err %d\n", err);
- return err;
-}
-
-static const struct ptp_clock_info ptp_dpaa2_caps = {
+static const struct ptp_clock_info dpaa2_ptp_caps = {
.owner = THIS_MODULE,
.name = "DPAA2 PTP Clock",
.max_adj = 512000,
.n_per_out = 3,
.n_pins = 0,
.pps = 1,
- .adjfreq = ptp_dpaa2_adjfreq,
- .adjtime = ptp_dpaa2_adjtime,
- .gettime64 = ptp_dpaa2_gettime,
- .settime64 = ptp_dpaa2_settime,
+ .adjfine = ptp_qoriq_adjfine,
+ .adjtime = ptp_qoriq_adjtime,
+ .gettime64 = ptp_qoriq_gettime,
+ .settime64 = ptp_qoriq_settime,
};
static int dpaa2_ptp_probe(struct fsl_mc_device *mc_dev)
{
struct device *dev = &mc_dev->dev;
- struct ptp_dpaa2_priv *ptp_dpaa2;
- u32 tmr_add = 0;
+ struct ptp_qoriq *ptp_qoriq;
+ struct device_node *node;
+ void __iomem *base;
int err;
- ptp_dpaa2 = devm_kzalloc(dev, sizeof(*ptp_dpaa2), GFP_KERNEL);
- if (!ptp_dpaa2)
+ ptp_qoriq = devm_kzalloc(dev, sizeof(*ptp_qoriq), GFP_KERNEL);
+ if (!ptp_qoriq)
return -ENOMEM;
err = fsl_mc_portal_allocate(mc_dev, 0, &mc_dev->mc_io);
goto err_free_mcp;
}
- ptp_dpaa2->ptp_mc_dev = mc_dev;
+ ptp_qoriq->dev = dev;
- err = dprtc_get_freq_compensation(mc_dev->mc_io, 0,
- mc_dev->mc_handle, &tmr_add);
- if (err) {
- dev_err(dev, "dprtc_get_freq_compensation err %d\n", err);
+ node = of_find_compatible_node(NULL, NULL, "fsl,dpaa2-ptp");
+ if (!node) {
+ err = -ENODEV;
goto err_close;
}
- ptp_dpaa2->freq_comp = tmr_add;
- ptp_dpaa2->caps = ptp_dpaa2_caps;
+ dev->of_node = node;
- ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev);
- if (IS_ERR(ptp_dpaa2->clock)) {
- err = PTR_ERR(ptp_dpaa2->clock);
+ base = of_iomap(node, 0);
+ if (!base) {
+ err = -ENOMEM;
goto err_close;
}
- dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock);
+ err = ptp_qoriq_init(ptp_qoriq, base, &dpaa2_ptp_caps);
+ if (err)
+ goto err_unmap;
- dev_set_drvdata(dev, ptp_dpaa2);
+ dpaa2_phc_index = ptp_qoriq->phc_index;
+ dev_set_drvdata(dev, ptp_qoriq);
return 0;
+err_unmap:
+ iounmap(base);
err_close:
dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
err_free_mcp:
static int dpaa2_ptp_remove(struct fsl_mc_device *mc_dev)
{
- struct ptp_dpaa2_priv *ptp_dpaa2;
struct device *dev = &mc_dev->dev;
+ struct ptp_qoriq *ptp_qoriq;
+
+ ptp_qoriq = dev_get_drvdata(dev);
- ptp_dpaa2 = dev_get_drvdata(dev);
- ptp_clock_unregister(ptp_dpaa2->clock);
+ dpaa2_phc_index = -1;
+ ptp_qoriq_free(ptp_qoriq);
dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
fsl_mc_portal_free(mc_dev->mc_io);
return mc_send_command(mc_io, &cmd);
}
-
-/**
- * dprtc_set_freq_compensation() - Sets a new frequency compensation value.
- *
- * @mc_io: Pointer to MC portal's I/O object
- * @cmd_flags: Command flags; one or more of 'MC_CMD_FLAG_'
- * @token: Token of DPRTC object
- * @freq_compensation: The new frequency compensation value to set.
- *
- * Return: '0' on Success; Error code otherwise.
- */
-int dprtc_set_freq_compensation(struct fsl_mc_io *mc_io,
- u32 cmd_flags,
- u16 token,
- u32 freq_compensation)
-{
- struct dprtc_get_freq_compensation *cmd_params;
- struct fsl_mc_command cmd = { 0 };
-
- cmd.header = mc_encode_cmd_header(DPRTC_CMDID_SET_FREQ_COMPENSATION,
- cmd_flags,
- token);
- cmd_params = (struct dprtc_get_freq_compensation *)cmd.params;
- cmd_params->freq_compensation = cpu_to_le32(freq_compensation);
-
- return mc_send_command(mc_io, &cmd);
-}
-
-/**
- * dprtc_get_freq_compensation() - Retrieves the frequency compensation value
- *
- * @mc_io: Pointer to MC portal's I/O object
- * @cmd_flags: Command flags; one or more of 'MC_CMD_FLAG_'
- * @token: Token of DPRTC object
- * @freq_compensation: Frequency compensation value
- *
- * Return: '0' on Success; Error code otherwise.
- */
-int dprtc_get_freq_compensation(struct fsl_mc_io *mc_io,
- u32 cmd_flags,
- u16 token,
- u32 *freq_compensation)
-{
- struct dprtc_get_freq_compensation *rsp_params;
- struct fsl_mc_command cmd = { 0 };
- int err;
-
- cmd.header = mc_encode_cmd_header(DPRTC_CMDID_GET_FREQ_COMPENSATION,
- cmd_flags,
- token);
-
- err = mc_send_command(mc_io, &cmd);
- if (err)
- return err;
-
- rsp_params = (struct dprtc_get_freq_compensation *)cmd.params;
- *freq_compensation = le32_to_cpu(rsp_params->freq_compensation);
-
- return 0;
-}
-
-/**
- * dprtc_get_time() - Returns the current RTC time.
- *
- * @mc_io: Pointer to MC portal's I/O object
- * @cmd_flags: Command flags; one or more of 'MC_CMD_FLAG_'
- * @token: Token of DPRTC object
- * @time: Current RTC time.
- *
- * Return: '0' on Success; Error code otherwise.
- */
-int dprtc_get_time(struct fsl_mc_io *mc_io,
- u32 cmd_flags,
- u16 token,
- uint64_t *time)
-{
- struct dprtc_time *rsp_params;
- struct fsl_mc_command cmd = { 0 };
- int err;
-
- cmd.header = mc_encode_cmd_header(DPRTC_CMDID_GET_TIME,
- cmd_flags,
- token);
-
- err = mc_send_command(mc_io, &cmd);
- if (err)
- return err;
-
- rsp_params = (struct dprtc_time *)cmd.params;
- *time = le64_to_cpu(rsp_params->time);
-
- return 0;
-}
-
-/**
- * dprtc_set_time() - Updates current RTC time.
- *
- * @mc_io: Pointer to MC portal's I/O object
- * @cmd_flags: Command flags; one or more of 'MC_CMD_FLAG_'
- * @token: Token of DPRTC object
- * @time: New RTC time.
- *
- * Return: '0' on Success; Error code otherwise.
- */
-int dprtc_set_time(struct fsl_mc_io *mc_io,
- u32 cmd_flags,
- u16 token,
- uint64_t time)
-{
- struct dprtc_time *cmd_params;
- struct fsl_mc_command cmd = { 0 };
-
- cmd.header = mc_encode_cmd_header(DPRTC_CMDID_SET_TIME,
- cmd_flags,
- token);
- cmd_params = (struct dprtc_time *)cmd.params;
- cmd_params->time = cpu_to_le64(time);
-
- return mc_send_command(mc_io, &cmd);
-}
projects / users / willy / linux.git / commitdiff