int ptp_clock_unregister(struct ptp_clock *ptp)
{
ptp->defunct = 1;
wake_up_interruptible(&ptp->tsev_wq);
/* Release the clock's resources. */
if (ptp->pps_source)
pps_unregister_source(ptp->pps_source);
ptp_cleanup_sysfs(ptp);
device_destroy(ptp_class, ptp->devid);
posix_clock_unregister(&ptp->clock);
return 0;
}
static void delete_ptp_clock(struct posix_clock *pc)
{
struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
/* Release the clock's resources. */
if (ptp->info->pps)
pps_unregister_source(ptp->pps_source);
ptp_cleanup_sysfs(ptp);
device_destroy(ptp_class, ptp->devid);
mutex_destroy(&ptp->tsevq_mux);
/* Remove the clock from the bit map. */
mutex_lock(&ptp_clocks_mutex);
clear_bit(ptp->index, ptp_clocks_map);
mutex_unlock(&ptp_clocks_mutex);
kfree(ptp);
}
struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
struct device *parent)
{
struct ptp_clock *ptp;
int err = 0, index, major = MAJOR(ptp_devt);
if (info->n_alarm > PTP_MAX_ALARMS)
return ERR_PTR(-EINVAL);
/* Find a free clock slot and reserve it. */
err = -EBUSY;
mutex_lock(&ptp_clocks_mutex);
index = find_first_zero_bit(ptp_clocks_map, PTP_MAX_CLOCKS);
if (index < PTP_MAX_CLOCKS)
set_bit(index, ptp_clocks_map);
else
goto no_slot;
/* Initialize a clock structure. */
err = -ENOMEM;
ptp = kzalloc(sizeof(struct ptp_clock), GFP_KERNEL);
if (ptp == NULL)
goto no_memory;
ptp->clock.ops = ptp_clock_ops;
ptp->clock.release = delete_ptp_clock;
ptp->info = info;
ptp->devid = MKDEV(major, index);
ptp->index = index;
spin_lock_init(&ptp->tsevq.lock);
mutex_init(&ptp->tsevq_mux);
init_waitqueue_head(&ptp->tsev_wq);
/* Create a new device in our class. */
ptp->dev = device_create(ptp_class, parent, ptp->devid, ptp,
"ptp%d", ptp->index);
if (IS_ERR(ptp->dev))
goto no_device;
dev_set_drvdata(ptp->dev, ptp);
err = ptp_populate_sysfs(ptp);
if (err)
goto no_sysfs;
/* Register a new PPS source. */
if (info->pps) {
struct pps_source_info pps;
memset(&pps, 0, sizeof(pps));
snprintf(pps.name, PPS_MAX_NAME_LEN, "ptp%d", index);
pps.mode = PTP_PPS_MODE;
pps.owner = info->owner;
ptp->pps_source = pps_register_source(&pps, PTP_PPS_DEFAULTS);
if (!ptp->pps_source) {
pr_err("failed to register pps source\n");
goto no_pps;
}
}
/* Create a posix clock. */
err = posix_clock_register(&ptp->clock, ptp->devid);
if (err) {
pr_err("failed to create posix clock\n");
goto no_clock;
}
mutex_unlock(&ptp_clocks_mutex);
return ptp;
no_clock:
if (ptp->pps_source)
pps_unregister_source(ptp->pps_source);
no_pps:
ptp_cleanup_sysfs(ptp);
no_sysfs:
device_destroy(ptp_class, ptp->devid);
no_device:
mutex_destroy(&ptp->tsevq_mux);
kfree(ptp);
no_memory:
clear_bit(index, ptp_clocks_map);
no_slot:
mutex_unlock(&ptp_clocks_mutex);
return ERR_PTR(err);
}
