blob: c89ddb63ef4ffe50050d66466653bee0717a50ed [file] [log] [blame]
/*
* switch_netlink.c: switch(netlink) set API
*
* Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netlink/netlink.h>
#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>
#include "switch_netlink.h"
static struct nl_sock *user_sock;
static struct nl_cache *cache;
static struct genl_family *family;
static struct nlattr *attrs[MT753X_ATTR_TYPE_MAX + 1];
static int wait_handler(struct nl_msg *msg, void *arg)
{
int *finished = arg;
*finished = 1;
return NL_STOP;
}
static int list_swdevs(struct nl_msg *msg, void *arg)
{
struct mt753x_attr *val = arg;
struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
if (nla_parse(attrs, MT753X_ATTR_TYPE_MAX, genlmsg_attrdata(gnlh, 0),
genlmsg_attrlen(gnlh, 0), NULL) < 0)
goto done;
if (gnlh->cmd == MT753X_CMD_REPLY) {
if (attrs[MT753X_ATTR_TYPE_MESG]) {
val->dev_info =
nla_get_string(attrs[MT753X_ATTR_TYPE_MESG]);
printf("register switch dev:\n%s", val->dev_info);
}
else {
fprintf(stderr, "ERROR:No switch dev now\n");
goto done;
}
} else
goto done;
return 0;
done:
return NL_SKIP;
}
static int construct_attrs(struct nl_msg *msg, void *arg)
{
struct mt753x_attr *val = arg;
int type = val->type;
if (val->dev_id > -1)
NLA_PUT_U32(msg, MT753X_ATTR_TYPE_DEV_ID, val->dev_id);
if (val->op == 'r') {
if (val->phy_dev != -1)
NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY_DEV, val->phy_dev);
if (val->port_num >= 0)
NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY, val->port_num);
NLA_PUT_U32(msg, type, val->reg);
} else if (val->op == 'w') {
if (val->phy_dev != -1)
NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY_DEV, val->phy_dev);
if (val->port_num >= 0)
NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY, val->port_num);
NLA_PUT_U32(msg, type, val->reg);
NLA_PUT_U32(msg, MT753X_ATTR_TYPE_VAL, val->value);
} else {
printf("construct_attrs_message\n");
NLA_PUT_STRING(msg, type, "hello");
}
return 0;
nla_put_failure:
return -1;
}
static int spilt_attrs(struct nl_msg *msg, void *arg)
{
struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
struct mt753x_attr *val = arg;
char *str;
int data;
if (nla_parse(attrs, MT753X_ATTR_TYPE_MAX, genlmsg_attrdata(gnlh, 0),
genlmsg_attrlen(gnlh, 0), NULL) < 0)
goto done;
if ((gnlh->cmd == MT753X_CMD_WRITE) || (gnlh->cmd == MT753X_CMD_READ)) {
if (attrs[MT753X_ATTR_TYPE_MESG]) {
str = nla_get_string(attrs[MT753X_ATTR_TYPE_MESG]);
printf(" %s\n", str);
if (!strncmp(str, "No", 2))
goto done;
}
if (attrs[MT753X_ATTR_TYPE_REG]) {
val->reg =
nla_get_u32(attrs[MT753X_ATTR_TYPE_REG]);
}
if (attrs[MT753X_ATTR_TYPE_VAL]) {
val->value =
nla_get_u32(attrs[MT753X_ATTR_TYPE_VAL]);
}
}
else
goto done;
return 0;
done:
return NL_SKIP;
}
static int mt753x_request_callback(int cmd, int (*spilt)(struct nl_msg *, void *),
int (*construct)(struct nl_msg *, void *),
void *arg)
{
struct nl_msg *msg;
struct nl_cb *callback = NULL;
int finished;
int flags = 0;
int err;
/*Allocate an netllink message buffer*/
msg = nlmsg_alloc();
if (!msg) {
fprintf(stderr, "Failed to allocate netlink message\n");
exit(1);
}
if (!construct) {
if (cmd == MT753X_CMD_REQUEST)
flags |= NLM_F_REQUEST;
else
flags |= NLM_F_DUMP;
}
genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, genl_family_get_id(family),
0, flags, cmd, 0);
/*Fill attaribute of netlink message by construct function*/
if (construct) {
if (construct(msg, arg) < 0) {
fprintf(stderr, "attributes error\n");
goto nal_put_failure;
}
}
/*Allocate an new callback handler*/
callback = nl_cb_alloc(NL_CB_CUSTOM);
if (!callback) {
fprintf(stderr, "Failed to allocate callback handler\n");
exit(1);
}
/*Send netlink message*/
err = nl_send_auto_complete(user_sock, msg);
if (err < 0) {
fprintf(stderr, "nl_send_auto_complete failied:%d\n", err);
goto out;
}
finished = 0;
if (spilt)
nl_cb_set(callback, NL_CB_VALID, NL_CB_CUSTOM, spilt, arg);
if (construct)
nl_cb_set(callback, NL_CB_ACK, NL_CB_CUSTOM, wait_handler,
&finished);
else
nl_cb_set(callback, NL_CB_FINISH, NL_CB_CUSTOM, wait_handler,
&finished);
/*receive message from kernel request*/
err = nl_recvmsgs(user_sock, callback);
if (err < 0)
goto out;
/*wait until an ACK is received for the latest not yet acknowledge*/
if (!finished)
err = nl_wait_for_ack(user_sock);
out:
if (callback)
nl_cb_put(callback);
nal_put_failure:
nlmsg_free(msg);
return err;
}
void mt753x_netlink_free(void)
{
if (family)
nl_object_put((struct nl_object *)family);
if (cache)
nl_cache_free(cache);
if (user_sock)
nl_socket_free(user_sock);
user_sock = NULL;
cache = NULL;
family = NULL;
}
int mt753x_netlink_init(void)
{
int ret;
user_sock = NULL;
cache = NULL;
family = NULL;
/*Allocate an new netlink socket*/
user_sock = nl_socket_alloc();
if (!user_sock) {
fprintf(stderr, "Failed to create user socket\n");
goto err;
}
/*Connetct the genl controller*/
if (genl_connect(user_sock)) {
fprintf(stderr, "Failed to connetct to generic netlink\n");
goto err;
}
/*Allocate an new nl_cache*/
ret = genl_ctrl_alloc_cache(user_sock, &cache);
if (ret < 0) {
fprintf(stderr, "Failed to allocate netlink cache\n");
goto err;
}
/*Look up generic netlik family by "mt753x" in the provided cache*/
family = genl_ctrl_search_by_name(cache, MT753X_GENL_NAME);
if (!family) {
//fprintf(stderr,"switch(mt753x) API not be prepared\n");
goto err;
}
return 0;
err:
mt753x_netlink_free();
return -EINVAL;
}
void mt753x_list_swdev(struct mt753x_attr *arg, int cmd)
{
int err;
err = mt753x_request_callback(cmd, list_swdevs, NULL, arg);
if (err < 0)
fprintf(stderr, "mt753x list dev error\n");
}
static int mt753x_request(struct mt753x_attr *arg, int cmd)
{
int err;
err = mt753x_request_callback(cmd, spilt_attrs, construct_attrs, arg);
if (err < 0) {
fprintf(stderr, "mt753x deal request error\n");
return err;
}
return 0;
}
static int phy_operate_netlink(char op, struct mt753x_attr *arg, int port_num,
int phy_dev, int offset, int *value)
{
int ret;
struct mt753x_attr *attr = arg;
attr->port_num = port_num;
attr->phy_dev = phy_dev;
attr->reg = offset;
attr->value = -1;
attr->type = MT753X_ATTR_TYPE_REG;
switch (op)
{
case 'r':
attr->op = 'r';
ret = mt753x_request(attr, MT753X_CMD_READ);
*value = attr->value;
break;
case 'w':
attr->op = 'w';
attr->value = *value;
ret = mt753x_request(attr, MT753X_CMD_WRITE);
break;
default:
break;
}
return ret;
}
int reg_read_netlink(struct mt753x_attr *arg, int offset, int *value)
{
int ret;
ret = phy_operate_netlink('r', arg, -1, -1, offset, value);
return ret;
}
int reg_write_netlink(struct mt753x_attr *arg, int offset, int value)
{
int ret;
ret = phy_operate_netlink('w', arg, -1, -1, offset, &value);
return ret;
}
int phy_cl22_read_netlink(struct mt753x_attr *arg, int port_num, int phy_addr, int *value)
{
int ret;
ret = phy_operate_netlink('r', arg, port_num, -1, phy_addr, value);
return ret;
}
int phy_cl22_write_netlink(struct mt753x_attr *arg, int port_num, int phy_addr, int value)
{
int ret;
ret = phy_operate_netlink('w', arg, port_num, -1, phy_addr, &value);
return ret;
}
int phy_cl45_read_netlink(struct mt753x_attr *arg, int port_num, int phy_dev,
int phy_addr, int *value)
{
int ret;
ret = phy_operate_netlink('r', arg, port_num, phy_dev, phy_addr, value);
return ret;
}
int phy_cl45_write_netlink(struct mt753x_attr *arg, int port_num, int phy_dev,
int phy_addr, int value)
{
int ret;
ret = phy_operate_netlink('w', arg, port_num, phy_dev, phy_addr, &value);
return ret;
}
void dump_extend_phy_reg(struct mt753x_attr *arg, int port_no, int from,
int to, int is_local, int page_no)
{
int ret;
int i = 0;
int temp = 0;
int r31 = 0;
if (is_local == 0) {
printf("\n\nGlobal Register Page %d\n",page_no);
printf("===============");
r31 |= 0 << 15; //global
r31 |= ((page_no&0x7) << 12); //page no
phy_cl22_write_netlink(arg, port_no, 31, r31); //select global page x
for (i = 16; i < 32; i++) {
if(i%8 == 0)
printf("\n");
phy_cl22_read_netlink(arg, port_no, i, &temp);
printf("%02d: %04X ", i, temp);
}
} else {
printf("\n\nLocal Register Port %d Page %d\n",port_no, page_no);
printf("===============");
r31 |= 1 << 15; //local
r31 |= ((page_no&0x7) << 12); //page no
phy_cl22_write_netlink(arg, port_no, 31, r31); //select global page x
for (i = 16; i < 32; i++) {
if (i%8 == 0) {
printf("\n");
}
phy_cl22_read_netlink(arg, port_no, i, &temp);
printf("%02d: %04X ",i, temp);
}
}
printf("\n");
}
int phy_dump_netlink(struct mt753x_attr *arg, int phy_addr)
{
int i;
int ret;
unsigned int offset, value;
if (phy_addr == 32) {
/*dump all phy register*/
for (i = 0; i < 5; i++) {
printf("\n[Port %d]=============", i);
for (offset = 0; offset < 16; offset++) {
if (offset % 8 == 0)
printf("\n");
ret = phy_cl22_read_netlink(arg, i, offset, &value);
printf("%02d: %04X ", offset, value);
}
}
} else {
printf("\n[Port %d]=============", phy_addr);
for (offset = 0; offset < 16; offset++) {
if (offset % 8 == 0)
printf("\n");
ret = phy_cl22_read_netlink(arg, phy_addr, offset, &value);
printf("%02d: %04X ", offset, value);
}
}
printf("\n");
for (offset = 0; offset < 5; offset++) { //global register page 0~4
if (phy_addr == 32) //dump all phy register
dump_extend_phy_reg(arg, 0, 16, 31, 0, offset);
else
dump_extend_phy_reg(arg, phy_addr, 16, 31, 0, offset);
}
if (phy_addr == 32) { //dump all phy register
for (offset = 0; offset < 5; offset++) { //local register port 0-port4
dump_extend_phy_reg(arg, offset, 16, 31, 1, 0); //dump local page 0
dump_extend_phy_reg(arg, offset, 16, 31, 1, 1); //dump local page 1
dump_extend_phy_reg(arg, offset, 16, 31, 1, 2); //dump local page 2
dump_extend_phy_reg(arg, offset, 16, 31, 1, 3); //dump local page 3
}
} else {
dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 0); //dump local page 0
dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 1); //dump local page 1
dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 2); //dump local page 2
dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 3); //dump local page 3
}
return ret;
}